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ABSTRACT 


Design  and  analysis  of  multiple  input  autopilots  using  sliding  modes  in  order  to 
achieve  accurate  horizontal  and  vertical  plane  control  of  an  autonomous  underwater 
vehicle  over  a  wide  variation  of  speeds  is  presented.  The  simulated  vehicle  is 
equipped  with  two  (fore  and  aft)  sets  of  dive  planes  and  two  sets  of  rudders.  In 
addition,  two  vertical  and  two  horizontal  thrusters  are  provided  for  control  during  low 
speed  or  hovering  operations.  The  entire  range  of  vehicle  speeds  from  zero  speed 
hovering  to  full  speed  ahead  is  divided  into  regions  depending  on  control  efficiency. 
Thrusters  are  used  for  low  speed  hovering,  control  surfaces  for  high  speed  cruising, 
and  a  combination  of  thrusters  and  control  surfaces  for  transition  speeds.  Linear 
quadratic  regulator  optimal  control  techniques  coupled  with  the  robustness  properties 
of  sliding  mode  control  are  utilized  to  provide  the  necessary  control  reversal  which 
occurs  during  the  transition  from  cruise  to  hover  mode.  Constant  disturbances  arising 
from  underwater  currents  are  effectively  compensated  resulting  in  accurate  path 
keeping.  As  a  consequence  of  the  multiple  input  control  methodology  developed  in 
this  work,  it  is  shown  that  both  path  and  orientation  accuracy  can  be  achieved  in 
moderate  cross  current  environments.  Finally,  reduced  order  observers  are  designed 
in  order  to  account  for  sensor  absence  or  malfunction. 
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I.  INTRODUCTION 


A.  GENERAL 

The  research  area  of  automatic  control  of  autonomous  underwater  vehicles,  or 
AUVs,  is  of  particular  interest  to  the  Navy  and  private  industry.  AUVs  are  capable 
of  a  variety  of  unclassified  missions  including  ASW,  decoy,  survey,  reconnaissance, 
and  ocean  engineering  work.  The  attractiveness  of  small  unmanned  vehicles  is 
increasing  due  to  the  escalating  capital  and  operational  costs  of  manned  submarines. 

The  dynamics  of  underwater  vehicles  are  described  by  highly  nonlinear  systems 
of  equations  with  uncertain  coefficients  and  disturbances  that  are  difficult  to  measure. 
An  automatic  controller  for  an  AUV  must  satisfy  two  conflicting  requirements:  First, 
it  must  be  sophisticated  enough  to  perform  its  mission  in  an  open  ocean  environment 
with  ever-changing  vehicle/environment  interactions.  Second,  it  must  be  simple 
enough  to  achieve  real-time  control  without  nonessential  computational  delays. 

Sliding  mode  control  theory  yields  a  design  that  fulfills  the  above  requirements. 
It  provides  accurate  control  of  nonlinear  systems  despite  unmodeled  system  dynamics 
and  disturbances.  Furthermore,  a  sliding  mode  controller  is  easy  to  design  and 
implement.  A  very  effective  pseudo-linear  sliding  mode  controller  can  be  developed 
from  the  linearized  equations  of  motion  for  an  underwater  vehicle. 

The  sliding  mode  control  concept  was  first  introduced  by  Utkin  [Ref.  1]  and 
recently  developed  by  Slotine  [Ref.  2].  At  the  U.S.  Naval  Postgraduate  School  (NPS), 
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the  sliding  mode  theory  has  been  applied  by  several  past  students  for  control  of  an 
AUV,  e.g.,  Lienard  [Ref.  3]  and  Joo-No  Sur  [Ref.  4].  However,  past  students  have 
approached  the  problem  of  AUV  control  as  a  single  input/single  output  (SISO) 
system.  So  far,  satisfactory  control  has  been  realized  with  this  method,  but  improved 
control  should  be  achievable  by  applying  multiple  input/multiple  output  (MIMO) 
sliding  mode  control  theory.  In  fact,  it  is  imperative  to  treat  an  AUV  as  a  MIMO 
system  to  be  able  to  control  it  through  the  entire  speed  range  from  hovering  at  zero 
speed  to  maximum  speed. 

B.  AIM  OF  THIS  STUDY 

The  aim  of  this  study  is  to  develop  a  robust  MIMO  sliding  mode  controller  that 
will  control  speed,  heading,  and  depth  through  a  wide  range  of  operational  speeds 
even  in  the  presence  of  underwater  currents. 

An  AUV  has  been  recently  constructed  at  NPS  and  is  currently  being  outfitted. 
The  hydrodynamic  coefficients  of  the  NPS  AUV  are  unknown,  hence  a  surrogate 
vehicle  with  known  characteristics  is  needed  to  explore  control  theory.  As  in  prior 
AUV  control  theses  at  NPS,  the  Mark  IX  Swimmer  Delivery  Vehicle  (SDV)  will  be 
utilized. 

The  first  step  of  this  study  is  the  development  of  the  linear  quadratic  regulator 
(LQR)  optimal  control  technique  for  MIMO  sliding  mode  controller  design.  This 
method  is  utilized  for  the  design  of  a  depth  controller.  In  the  vertical  plane,  the  full 
nonlinear  equations  of  motion  are  simplified  until  they  become  decoupled  from  the 


2 


horizontal  plane  equations.  The  MIMO  sliding  mode  depth  controller  is  designed 
and  simulated  based  on  these  simplified  nonlinear  equations. 

The  LQR  method  is  then  utilized  for  the  design  of  a  line-of-sight  (LOS)  steering 
controller  and  then  a  cross-track-error  (CTE)  steering  controller.  These  controllers 
are  also  designed  and  simulated  based  on  simplified  nonlinear  equations  of  motion. 

The  diving  and  steering  (first  the  LOS  and  then  the  CTE)  controllers  are  then 
combined  into  a  control  package.  This  package  is  then  used  to  control  the  AUV 
utilizing  the  full  nonlinear  equations  of  motion. 

Finally,  three  dimensional  path  keeping  is  developed.  This  method  uses  two 
cross-track-error  controllers  to  allow  the  AUV  to  follow  a  straight  line  path  in  thr^.e 
dimensional  space. 

In  all  cases,  reduced  order  observers  are  designed  to  estimate  states  that  are  not 
directly  measurable.  Additionally,  the  speed  controller  developed  by  Lienard  [Ref.  3] 
was  utilized  in  all  cases. 

C.  THESIS  OUTLINE 

In  Chapter  II,  the  LQR  method  of  MIMO  sliding  mode  controller  design  is 
developed.  Chapter  III  discusses  the  diving  controller  design  and  simulation. 
Chapter  IV  is  concerned  with  the  LOS  steering  controller.  In  Chapter  V,  the  CTE 
steering  controller  is  developed.  Simulation  of  the  AUV  with  the  full  nonlinear 
equations  of  motion  and  the  combined  control  package  is  achieved  in  Chapter  VI. 
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In  addition,  three  dimensional  path  keeping  is  developed  and  simulated  in  Chapter 
VI.  Finally,  Chapter  VII  contains  the  conclusions  and  recommendations. 
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II.  THEORY  OF  MIMO  SLIDING  MODE  CONTROL 


A.  INTRODUCTION 

This  chapter  is  devoted  to  the  theory  of  MIMO  sliding  mode  control.  A  more 
thorough  discussion  of  sliding  mode  theory  can  be  found  in  [Ref.  1]  and  [Ref.  2].  In 
this  chapter,  sliding  mode  theory  is  reviewed  with  emphasis  on  the  differences  due 
to  MIMO  applications.  Then,  general  procedures  for  designing  a  sliding  mode 
MIMO  controller  are  delineated. 

B.  REVIEW  OF  SLIDING  MODE  THEORY 

1.  Overview  of  MIMO  Sliding  Mode  Control  Equations 

The  controller  design  process  begins  with  the  linearized  equations  of 
motion  written  in  the  standard  state  space  representation  as  follows: 

of  =  Ax  +  Bu  (2-1) 

where,  x  e  R°,  state  vector 

u  e  Rm,  control  input  vector 
A  e  Rnxn,  state  matrix 
B  e  Rnxin,  control  distribution  matrix 
n,  number  of  states 
m,  number  of  inputs 
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The  sliding  mode  control  laws,  u,  for  the  system  of  (2.1)  are  of  the  form: 

u  =  u  +u  (2-2) 

As  can  be  seen  in  (2.2),  the  control  laws  are  composed  of  two  parts.  It  will  be  seen 
later  that  the  first,  u,  are  linear  feedback  laws  based  on  the  nominal  linearized  model 
(2.1).  The  second,  u ,  are  nonlinear  feedbacks  with  their  signs  switching  between  plus 
and  minus  according  to  the  location  of  the  system  with  respect  to  the  sliding  surfaces: 

<j(x)  =  Sr  x  =  0  (2*^) 

where,  S  e  Rox,n 
a  e  Rm 

Determination  of  S  will  determine  the  sliding  surfaces  uniquely. 

The  control  laws  (2.2)  must  be  able  to  drive  the  system  (2.1)  onto  the 
sliding  surfaces  (2.3)  and  the  nominal  operation  point  for  an  arbitrary  choice  of  initial 
conditions.  It  will  be  seen  later  that  the  dimension  of  the  m  sliding  surfaces  must  be 
one  less  than  the  dimension  of  the  state  space  since  u  has  to  change  sign  as  the 
system  crosses  a(x )  =  0.  Detailed  development  of  u  and  u  is  in  the  sections  that 
follow.  [Ref.  5] 
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2.  Liapunov  Function 


By  defining  the  Liapunov  function: 

v(x)  =  ^[o(x)]2  =  l(a]  +  a\  +  ...  +  a2m  )  (2.4) 

asymptotic  stability  of  (2.3)  is  guaranteed  provided  V{x)  is  a  negative  definite 
function: 


V(x)  =  o  [al  +  a2a2  +  ...  +  a  m  am  <  0 

This  is  true  if: 


(2.5) 


alal  <  0 
o,a.,  <  0 


a  o  •<  0 

m  m 


which  can  be  written  in  the  form: 


(2.6) 


=  -n  i  ) 

a 2  =  -n  \  Sign(a2 ) 


(2.7) 


=  -n2m  sign(°m  ) 
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3.  Determination  of  the  Control  Laws 

Using  (2.1)  and  (2.3)  in  (2.7)  results  in: 

ST(Ax  +  Bu)  =  -ka 

’ 

n]  sign  (a i) 
n22sign(a2) 

where,  k_  = 


h  m  sig*1  (°m  )| 


Solving  for  u: 


u  =  (Sr  B)  ' Sr .4x  -  (Sr  B)  ' k 


which  is  the  same  form  as  (2.2)  with: 


u  =  (ST#)  'ST/ir 


u  =  (5Tfi)  ’it 


(2.10) 

(2.11) 


When  on  the  sliding  surfaces  a(x)  =  0,  the  control  laws  become  u  =  u. 
Substituting  (2.10)  into  the  system  (2.1)  yields: 


x  =  [/4-fi(5TB)'‘ST/4^ 


(2.12) 


which  can  be  written: 


x  =  (A  -  Bk)x 


(2.13) 


8 


where,  k  =  (ST B)~l  ST A 


The  gain  matrix,  A,  can  be  found  from  standard  pole  placement  or  LOR 
methods.  The  closed  loop  dynamics  matrix: 

Ac=A-Bk  (2.14) 

has  eigenvalues  specified  for  desirable  response.  Additionally,  m  poles  of/ic  must  be 
zero  which  is  consistent  with  the  decomposition  (2.9). 

The  linear  feedbacks  u  provide  the  desired  dynamics  on  the  sliding 
surfaces  only.  Therefore,  u  has  no  effect  for  the  m  excursions  off  the  sliding  surfaces 
a(x)  =  0. 

Conversely,  the  nonlinear  feedbacks  u  only  drive  the  system  onto  the 
sliding  surfaces  and  provide  no  control  action  on  the  sliding  surfaces.  The  one 
requirement  for  this  action  is  the  gains  p,2  have  to  be  chosen  large  enough  so  u  can 
provide  the  required  robustness  due  to  momentary  disturbances  and  unmodeled 
dynamics  without  any  compromise  in  stability. 

4.  Determination  of  the  Sliding  Surfaces 

With  Ac  and  k  specified,  S  can  be  determined  as  follows.  Rearranging  the 
expression  for  k  in  (2.13)  and  using  the  expression  for  Ac  in  (2.14)  yields: 

St/4c  =  0  (2-15) 
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The  existence  of  m  linearly  independent  nontrivial  solutions  of  (2.15)  is  guaranteed 
if  the  closed  loop  dynamic  matrix  has  rank  deficiency  m,  i.e.,  if  Ac  has  m  poles  at  the 
origin.  This  is  consistent  with  the  decomposition  (2.9).  The  linear  feedbacks^ 
provide  the  desired  dynamics  on  the  m  sliding  surfaces  only.  Therefore,  &  has  no 
effect  in  the  directions  that  are  perpendicular  to  the  sliding  planes.  However,  it  is 
not  always  possible  to  compute  S  from  (2.15)  directly  since  the  existence  of  m  left 
eigenvectors  of  Ac  corresponding  to  the  multiplicity  m  zero  eigenvalue  is  not  always 
guaranteed.  Thus,  another  method  is  required  for  MIMO  sliding  mode  controller 
design.  This  leads  us  to  a  method  proposed  by  Utkin. 


C.  ALTERNATIVE  APPROACH 

In  [Ref.  6],  Utkin  proposed  an  alternative  method  of  sliding  mode  controller 
design  especially  for  MIMO  applications.  This  method  requires  a  transformation  of 
the  state  vector  and  associated  matrices. 

I.  Transformations 

a.  Transformation  Matrix 

First,  an  orthogonal  nxn  transformation  matrix,  T,  is  found  such  that: 


TB  = 


(2.16) 


where,  Bx  e  Rmxm 


0  e  Rt"-®)1® 
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QR  factorization  which  is  available  on  software  programs  such  as  Matrix-x  (Copyright 
1989  by  Integrated  Systems  Inc.)  can  be  used  to  determine  T.  After  QR 
factorization,  the  B  matrix  is  transformed  to: 


B-Q 


R 

0 


(2.17) 


where  Q  is  orthogonal  and  R  is  upper  triangular.  Rearranging  (2.17)  yields: 


where  then,  T  =  Q1 
Bt  =  R 


(2.18) 


Note  that  the  transformation  need  not  be  applied  if  the  B  matrix  is  already  of  the 
desired  form  of  (2.16).  This  was  found  to  be  the  situation  for  all  cases  of  MIMO 
control  of  the  SDV. 

b.  Transformed  Equations  of  Motion 

After  the  transformation,  the  new  states  are: 


y  =  Tx 


(2.19) 


and  the  system  (2.1)  becomes: 

y  =  TATry  +  TBu  (120) 
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which  can  be  decomposed  into  the  form: 


yi=Anyi+Any2+Biu 

y 2  =  ^21-^1  +  ^22-^2 

where,  £  Rm 

y2  e  R(D-“) 

An  £  Rmim 
A12  £  R“*(n-”) 

A21  £  R(n'n')M 
^22  £  *(■-■>*<■'■> 

Bx  £  Kmxm 

c.  Transformed  Sliding  Surfaces 

Using  the  transformation  of  the  states  (2.19)  in  the  equation  of  the 
sliding  surfaces  (2.3)  results  in: 

a(y) =  c1  y =  0  (123) 

where,  C  =  TS 

Equation  (2.23)  can  be  decomposed  to: 

*0)  =  c?yx  +  c2Tyz  = 0  (2-24) 

where,  C,T  c  R“,m 


(2.21) 

(2.22) 
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CT  e  Rm*(n-m) 


0  £  Rm 

Without  loss  of  generality,  the  matrix  Cx  can  be  set  equal  to  the  identity  matrix  /  to 
simplify  the  equations  for  the  sliding  surfaces. 

d.  Transformed  Liapunov  Function 

As  before,  a  Liapunov  function  is  defined  to  ensure  asymptotically 
stable  sliding  surfaces.  In  transformed  states,  the  Liapunov  function  is: 

m  =  1[°0)]2  <2-25> 


which  again  leads  to  (2.7). 


e.  Transformed  Control  Laws 

Substituting  (2.24)  into  (2.7)  leads  to  the  following  expression: 


*  +  C2Ty2  = 


(2.26) 


Substituting  (2.21)  and  (2.22)  into  (2.26)  and  solving  for  u  gives: 


u  =  -Bl-i[(An+C^An)y1  +  (Au+ 


(2.27) 


Again,  u  has  the  form  of  (2.2)  with: 


U  =  -«,"1[(i4„+  C2TA21)y1  +  (An+  C2TA22)y2] 

“  =  -K'K 
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(2.28) 

(2.29) 


2.  Transformed  Equations  on  the  Sliding  Surfaces 

When  the  system  is  on  the  sliding  surfaces,  the  control  laws  become, 
u  =  u,  and  the  sliding  surfaces  after  solving  fory^  are: 

*  =  -C2TJ2  (2-30) 

Using  (2.28)  and  (2.30)  in  (2.21),  the  first  set  of  system  equations  for  on  the 
sliding  surfaces  reduces  to: 


-C2t/=  -C2t(^1  +  ^2)  (2.31) 

This  is  nothing  more  than  a  linear  combination  of  the  second  set  of  system  equations 
(2.22)  for  y2\  Thus,  there  are  only  (n-m)  independent  equations  on  the  sliding 
surfaces  which  gives  m  poles  at  the  origin  as  before.  The  (n-m)  independent 
equations  on  the  sliding  surfaces  are: 

y2  =  (A22'A21C2T)y2 

3.  LQR  Method  for  Determination  of  the  Sliding  Surfaces 

In  [Ref.  6],  Utkin  discusses  pole  placement  and  LQR  methods  for 
determining  C2T.  Both  methods  have  been  utilized  for  the  control  of  the  SDV  during 
this  study.  The  theory  of  pole  placement  is  well  known  and  will  not  be  restated  here. 

For  the  LQR  method,  it  is  desired  to  minimize  the  quadratic  performance 

index: 
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(2.33) 


I  = 


where,  Q  >  0 

qt  =  q 

t0  =  time  sliding  mode  begins 


Partition  the  transformed  weighting  matrix  such  that: 


TQTr 


Using  the  partitioned  matrix  (2.34)  in  (2.33)  yields: 

1  =  +y*rQ*'y'  +y?QKy2 

+  ^2  622-^2^ 


or: 


1  =  ^JTo<^2Te*->’2  + 

where,  Q'  b  q22  -  Q2l  Qn  l  Qn 
A  —  A 22  -  ^21  2ti  1  C12 
*  Byi  +  fin"1  Q\2  yi 


(2.34) 


(2.35) 


(2.36) 
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Equation  (2.36)  is  now  a  recognizable  form  for  applying  the  LQR  technique.  One 
more  step  remains;  the  new  definitions  need  to  be  applied  to  the  system  which  is  on 
the  sliding  surfaces.  After  these  substitutions  (2.32)  converts  to: 

y2=A‘y2+A2iv  '2,3?) 

The  alterations  are  complete  and  the  problem  is  now  to  minimize  (2.36) 
subject  to  (2.37).  The  Hamiltonian  for  this  problem  is: 

H  =Pt(A-y1  *  A„ ,)  -  i&-2Te>2  ♦  vTe,^)  (2.38) 

and  the  algebraic  Riccati  equation  is: 

AJk  +  kA  -  kAnQn  'A2lTk  +  2*  =  0  (2-39) 

which  results  in  the  solution: 

v  =  Qn'A2?ky2  (2.40) 

Again  the  new  definitions  from  (2.36)  are  applied  but  in  reverse  to  return 

to  the  original  matrix  variables.  This  produces  the  desired  result: 


c2r  -  Qn'(Qu  +  AnTk) 


(2.41) 


4.  Determination  of  the  Control  Laws 


After  determining  C2T  by  pole  placement  or  by  the  LOR  method  above, 
calculation  of  the  control  laws  is  trivial  by  substitution  into  (2.28)  and  (2.29). 

D.  CONTROL  OF  CHATTERING 

Although  sliding  mode  is  a  robust  means  of  control  in  the  presence  of 
parameter  variations  and  input  disturbances,  it  has  an  inherent  chattering  problem. 
This  is  caused  by  the  discontinuity  of  the  nonlinear  feedbacks,  u.  As  stated  earlier, 
these  are  switching  terms  with  their  signs  switching  between  plus  and  minus  according 
to  the  location  of  the  system  with  respect  to  the  sliding  surfaces,  such  as: 


sign{oJ  = 


rl  ,  a  >  0 

- 

-1  ;  cr  <  0 


(2.42) 


The  chattering  problem  is  eliminated  by  introducing  small  boundary  layers  of 
thickness  <pl  around  each  sliding  surface.  Due  to  the  sliding  surfaces  being  defined 
via  a  Liapunov  function,  the  system  is  guaranteed  to  move  into  the  boundary  layers 
towards  the  sliding  surfaces.  Unfortunately,  the  dynamics  of  the  system  trajectory 
inside  the  boundary  layers  are  only  an  approximation  to  the  desired  dynamics  on  the 
sliding  surfaces.  The  advantage  of  the  scheme  is  that  the  trajectory  will  not  chatter 
close  to  the  sliding  surfaces. 


17 


The  scheme  is  carried  out  by  defining  a  saturation  function  in  place  of  sign^): 


+  1  ;  a  > 


<t>, 


satsgnioj 


-1  ;  -4>i  <  o.  <  <pt 

*  i 


[-1  ;  a.  <  -<f>. 


(2.43) 


Theoretically,  each  sliding  surface  can  have  a  different  boundary  layer  thickness; 
however,  during  this  study  the  same  boundary  layer  thickness  was  used  for  all  sliding 
surfaces  of  a  given  system. 


E.  COMMENTS 

Methods  for  designing  a  MIMO  sliding  mode  controller  have  been  discussed. 
To  review  Utkin’s  method,  the  sliding  surfaces  can  be  determined  by  pole  placement 
or  by  the  LQR  technique.  Both  of  these  methods,  in  the  most  general  case,  require 
a  transformation  of  the  states.  As  previously  discussed,  in  this  study  the  B  matrix  was 
always  in  the  correct  form  and  transformation  was  never  needed. 

Although  pole  placement  has  the  advantage  of  direct  placement  of  the  sliding 
surface  poles,  its  disadvantage  is  a  lack  of  direct  control  over  the  control 
methodology,  i.e.,  with  what  hierarchy  will  the  states  be  minimized.  As  will  be  seen 
later,  this  can  be  of  great  importance  for  AUV  control  and  is  the  motivation  for  using 
the  more  complex  LQR  method  when  conditions  warrant. 
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III.  MIMO  SLIDING  MODE  CONTROL  OF  THE  AUV  IN  THE  DIVE  PLANE 


A.  NONLINEAR  GOVERNING  EQUATIONS  IN  THE  DIVE  PLANE 

I.  Modifications  to  the  SDV  Equations  of  Motion 

As  stated  in  Chapter  I,  the  Mark  IX  SDV  has  been  used  in  this  thesis  for 
theoretical  study  of  AUV  dynamics;  however,  the  SDV  has  several  dissimilarities  with 
the  newly  constructed  NPS  AUV.  First,  the  SDV  has  a  third  propeller  mounted 
underneath  for  surface  operations.  Second,  the  SDV  has  no  vertical  or  horizontal 
thrusters  for  hovering  or  low  speed  operations.  Finally,  the  SDV  has  stern  rudders 
only.  [Ref.  7] 

The  SDV  model  from  [Ref.  7]  was  modified  to  approximate  the  AUV 
geometry  as  closely  as  possible.  First,  in  the  equations  of  motion  the  terms  for  the 
"extra"  third  propeller  were  dropped.  Second,  for  diving  operations  vertical  tunnel 
thrusters  were  added  to  the  model.  Finally,  a  set  of  bow  rudders  were  added  to  the 
model  for  MIMO  steering  control.  Steering  will  be  discussed  in  Chapter  IV. 

a.  Tunnel  Thrusters 

Several  decisions  had  to  be  made  in  adding  tunnel  thrusters  to  the 
SDV  model.  Commensurate  with  the  size  of  the  SDV,  it  was  decided  that  the  two 
vertical  tunnel  thrusters  would  have  a  maximum  thrust  of  5  lbf  each,  and  they  would 
be  located  symmetrically  on  each  side  of  the  origin  of  the  body  axis  coordinate  system 
a  longitudinal  distance  of  6.7  ft.  It  was  also  assumed  that  current  would  be  used  to 
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control  the  thrusters  where  +20  amperes  would  correspond  to  the  maximum  thrust 
of  5  lbf  in  the  positive  heave  direction  (downward).  The  relationship  between 
thruster  current  and  the  resultant  thrust  was  assumed  to  be  linear  [Ref.  8]. 

The  thrusters  were  assumed  to  be  infinitely  responsive,  i.e.,  zero 
response  time.  This  is  a  realistic  assumption  for  small  thrusters. 

The  effect  of  vehicle  surge  velocity  on  resultant  thrust  is  unknown  for 
very  small  tunnel  thrusters  and  is  the  topic  of  future  studies  at  NPS.  From  studies 
of  thrusters  in  general,  it  is  known  that  thrust  will  decrease  with  increasing  surge 
velocity  [Ref.  9].  It  was  assumed  during  this  study  that  the  relationship  was  linear 
with  a  thrust  degradation  of  20%  at  maximum  surge  velocity. 

Additionally,  the  effect  of  the  thrusters  on  the  surge  velocity  is  also 
unknown  and  was  unmodeled  in  this  study. 

2.  Identification  of  Symbols 

The  six  degree  of  freedom  equations  of  motion  for  an  underwater  vehicle 
are  usually  described  using  a  body  fixed  coordinate  system  and  an  inertial  reference 
frame.  Vehicle  position  is  expressed  in  Cartesian  coordinates  x,y,  and  z.  Orientation 
of  the  coordinate  system  is  expressed  in  Euler  angles  <p,  0,  and  t|r.  Hydrodynamic 
force  components  along  the  body  axes  are  expressed  as  X,  Y,  and  Z.  Hydrodynamic 
moment  components  along  the  body  axes  are  expressed  as  K,  M,  and  N.  Figure  8 
of  [Ref.  4]  shows  the  positive  directions  of  forces,  moments,  motions,  and  control 
surface  deflections.  The  definitions  of  the  states,  controls,  and  other  variables  are 
listed  in  Tables  1,  2,  and  3,  respectively. 
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Table  1.  DEFINITION  OF  AUV  STATES 


STATE 

DEFINITION 

UNITS 

u 

surge  velocity 

ft/s 

V 

sway  velocity 

ft/s 

w 

heave  velocity 

ft/s 

p 

roll  rate 

rad/s 

R 

pitch  rate 

rad/s 

r 

yaw  rate 

rad/s 

<P 

roll  angle 

radians 

e 

pitch  angle 

radians 

* 

yaw  angle 

radians 

Table  2.  DEFINITION  OF  AUV  CONTROLS 


CONTROL 

DEFINITION 

UNITS 

<5br 

bow  rudder  angle 

radians 

<5sr 

stern  rudder  angle 

radians 

bow  plane  angle 

radians 

5sp 

stern  plane  angle 

radians 

4v 

vert  bow  thruster  current 

amperes 

4v 

vert  stern  thruster  current 

amperes 

N 

propeller  speed 

rpm 
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Table  3.  DEFINITION  OF  ADDITIONAL  VARIABLES 


VARIABLE 

DEFINITION 

UNITS 

W 

vehicle  weight 

lbf 

B 

buoyant  force  on  vehicle 

lbf 

X 

distance  from  the  origin 

ft 

b(x) 

vehicle  beam  at  position  x 

ft 

h(x) 

vehicle  height  at  position  x 

ft 

xv  y r 

location  of  CG 

ft 

XB’  ZB 

location  of  CB 

ft 

UJx) 

cross  flow  velocity  at  x 

ft/s 

L 

vehicle  length 

ft 

p 

density  of  medium 

slugs/ft3 

V 

kinematic  viscos.  of  medium 

ft'/s 

m 

vehicle  mass 

slugs 

moment  of  inertia 

slug-ft2 

3.  Assumptions 

The  following  assumptions  have  also  been  made  for  the  diving  model: 

•  Vehicle  motion  is  confined  to  the  vertical  plane: 

(p,  />,  v,  v,  <p,  i|f,  r,  r,  6br,  6sr  =  0) 

•  The  AUV  is  neutrally  buoyant: 

(W  =  B) 

•  The  CB  and  CG  are  on  the  vehicle’s  centerline: 

Ov  =  °) 
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•  The  AUV  is  restricted  to  the  speed  range: 


(-6.0  ft/s  <  u  <  +6.0  ft/s) 

(-500  rpm  <  N  <  +500  rpm) 

•  The  planes  have  a  restricted  range  of  motion: 

6.  <  0.4  radians 

bp 

6  <  0.4  radians 

sp 

•  Surge  velocity  can  be  assumed  constant  in  the  heave  and  pitch  equations: 

(u  =  0) 

These  assumptions  greatly  simplify  the  governing  nonlinear  equations. 

4.  Resulting  Equations 

After  all  modifications  and  assumptions,  the  nonlinear  governing  equations 
from  [Ref.  7]  simplify  to: 


•  Normal  (Heave)  equation  of  motion 
m(w  -uq  -  xgq  -  zgq2)  =  |i.4Z^ 

♦  £/.j(Z>  +  Z',«9) 

.iL2[Ziuw.u\Z'Sip6bp>Z'ttp6lp)) 

p  pbow  CDzb(x)(w  -xqf^ 

2  J  stern  Ucf(x) 

+  (0.25  -  0.0083  «)/bv 

+  (0.25  -  0.0083  w)/sv 


(3.1) 
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•  Pitch  equation  of  motion 


fy?  -"4 xg(*-uq)  -  zwq ]  = 

+  ^L4(a4>v  +M'<iuq) 

+  +  w'W4bp6bp  +  A^sp<5sp)] 

+  P  rb°w  ^Dz  ft(r)(w-xq)3 
2  J  stem  £/cf(x)  X 

-  (x  IP  -*Bfi)cos0 

-  (^gW'-Zg^jsine 

-  6.7(0.25  -  0.0083 w)/bv 
+  6.7(0.25  -  0.0083 «)/sv 


(3.2) 


•  Surge  equation  of  motion 
m(a  +  wq  -xgq2  +zgq)  = 

+  fL3[*^  +  «^q4bp5bp  +yq6$p5sp)] 

+  |L2[Xw^2  +«»v(yw8bp6bp  +<4sp6sp)] 

+  ■|^2“2(^6bp«bp<5bp  +^fi5p«5p6s"p) 

+  fL2“2^prop 


24 


•  Kinematic  equations 


Q  =  q 


(3.4) 


z  =  w  -u  sin0 


(3.5) 


•  Propulsion  equations 

*Prop  =  cdo(7  I  n  I  -  1] 

CD0  =  0.00385  +  1.296 xl0-17(/?e  -  1.2xl07)2 

Re  =  —  ;  Reynold's  number 


u 

ud  =  0.012  N  ;  desired  surge  velocity 

•  Cross  flow  velocity  equation 

Ucf(x)  =  \(w-xq)\ 


(3.6) 

(3.7) 

(3.8) 

(3.9) 

(3.10) 

(3.11) 


B.  CONTROL  METHODOLOGY 

Previous  theses  at  NPS  have  attempted  to  control  the  AUV  only  at  high  speeds 
in  the  dive  plane,  e.g.,  [Ref.  4].  Problems  arise  at  low  speeds  with  only  planes  as 
control  inputs.  In  fact,  depth  of  the  neutrally  buoyant  AUV  cannot  be  controlled  at 
speeds  less  than  approximately  1.0  ft/s  because  the  hydrodynamic  forces  on  the  planes 
become  negligible.  This  control  problem  can  be  overcome  by  the  addition  of 
thrusters;  however,  this  poses  new  control  problems.  With  two  sets  of  planes  and  two 
thrusters,  the  system  now  has  four  control  inputs!  The  problem  arises  as  to  how 
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these  inputs  should  be  utilized  for  efficient  vehicle  control.  Figure  1  compares  the 
hydrodynamic  forces  produced  by  the  planes  and  thrusters  versus  surge  velocity. 
Obviously,  at  very  low  speeds  the  planes  are  or  little  use.  At  high  speeds,  the  thrust 
produced  by  the  tunnel  thrusters  is  negligible  in  comparison  to  the  planes. 

For  efficient  control,  it  was  decided  to  use  different  control  laws  at  different 
surge  velocities.  The  surge  velocity  u  was  partitioned  into  three  distinct  regions: 

•  Hover  region:  0.00  s  «  £  0.75  ft/s 

•  Transition  region:  0.75  <  u  <  1.75  ft/s 

•  Cruise  region:  1.75  <  u  <  6.00  ft/s 

In  the  hover  region,  only  thrusters  are  used  for  vehicle  control.  In  the  transition 
region,  thrusters  and  planes  are  used.  Finally,  in  the  cruise  region  planes  are  utilized. 
Thus,  there  are  unique  control  laws  and  sliding  surfaces  in  each  region. 

The  simplified  nonlinear  governing  equations  from  the  previous  section  are 
linearized  at  the  midpoint  of  each  speed  region,  u  =  0.375,  1.25,  3.875  ft/s,  to  obtain 
the  linea  zed  state  space  representation  of  (2.1)  for  controller  design.  Due  to  the 
complexity  of  the  heave  (3.1)  and  pitch  (3.2)  equations,  a  Fortran  program  was 
written  to  assist  in  their  solution.  The  program  is  named  SDVDIVLIN_I.FOR  (see 
Appendix  A).  The  program  solves  the  heave  and  pitch  equations  for  w  and  q. 
Program  output  is  expressed  as  two  highly  nonlinear  equations: 
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o  BOW  PLANES 
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U  (FT/S) 


Figure  1.  Forces  Produced  by  Planes  and  Thrusters  vs  Surge  Velocity 
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(3.12) 


ix(l)  =  M  'f WO,  «(/)) 

at 

where,  A/  =  "mass"  matrix 

The  heave  and  pitch  equations  are  then  linearized,  as  is  kinematic  equation  (3.5). 
With  the  inclusion  of  the  linear  kinematic  definition  (3.4),  the  resultant  system  has 
the  form  of  (2.1): 


w 

w 

<7 

e 

-M 

<7 

6 

♦M 

5sp 

7bv 

z 

z 

7SV 

(3.13) 


The  sliding  surfaces  and  control  laws  of  the  AUV  are  then  based  on  this  linear 
system. 


C.  CONTROL  OF  THE  AUV  IN  THE  HOVER  REGION 
1.  State  Space  Equations 

To  reiterate,  in  the  hover  region,  0.00  £  u  £  0.75  ft/s,  the  planes  are  not 
used  and  the  nonlinear  system  is  linearized  at  u  —  0.375  ft/s  to  determine  the  sliding 
surfaces  and  control  laws.  The  resulting  linear  state  space  representation  is: 


VV 

-0.0227 

-0.0505 

0.0251 

0.0 

w 

0.00017 

0.00014 

<7 

0.0056 

-0.0627 

-0.0668 

0.0 

<7 

-0.00005 

0.00004 

4v 

e 

0.0 

1.0 

0.0 

0.0 

6 

+ 

0.0 

0.0 

7SV. 

z 

1.0 

0.0 

-0.3750 

0.0 

z 

0.0 

0.0 

(3.14) 
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Note  the  B  matrix  has  the  required  form  of  (2.16)  and  the  transformation  matrix  T 
discussed  in  Chapter  II  is  not  needed,  or  T  can  be  thought  of  as  the  identity  matrix. 

Using  Utkin’s  method  of  MIMO  sliding  mode  controller  design,  (3.14)  can 
be  expressed  in  the  form  of  (2.21)  and  (2.22)  with: 


A 


li 


A 
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-0.0227  -0.0505 

= 

0.0251  0.0 

0.0056  -0.0627 

12 

-0.0668  0.0 

0.0  1.0 

A„  = 

0.0  0.0 

1.0  0.0 

22 

-0.3750  0.0 

0.00017 

-0.00005 


0.00014 

0.00004 


(3.15) 


-V,  =  lw  «r  y2  =  [0  ZY 


The  MIMO  sliding  mode  controller  will  be  based  on  this  linear  system. 

2.  Controller  Design  by  Pole  Placement 

With  pole  placement,  the  first  step  is  to  place  the  poles  of  the  system  on 
the  sliding  surfaces  to  determine  C2T: 


POLEPLACE^-A^}  (316) 

The  reader  is  reminded  that  this  method  requires  CjT  to  be  set  equal  to  the  2x2 
identity  matrix  as  discussed  in  Chapter  II. 
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A  problem  arises  because  C2T  is  a  2x2  matrix  with  four  unknowns,  while 
(3.16)  turns  out  to  be  a  simple  quadratic.  Thus,  there  are  two  equations  with  four 
unknowns.  This  is  a  major  disadvantage  of  using  pole  placement  for  MIMO  systems; 
there  are  more  unknowns  than  equations  which  precludes  a  straight  forward  use  of 
Matrix-x  to  easily  calculate  the  sliding  surfaces.  The  problem  can  be  overcome  by 
additional  constraints  on  the  eigenvectors  or  by  assuming  relationships  between  the 
elements  of  C2T.  Here  we  arbitrarily  assumed  the  two  relationships: 


C2(l,l)  =  0.5  C,(l,2) 
C,(2,l)  =  0.5  C2(2,2) 


(3.17) 


Additionally,  we  pick  the  two  poles  at  -0.50  and  -0.51.  C2T  can  now  be  determined: 


0.6750  -0.3400 
1.3500  -0.6800 


With  (3.18),  the  two  sliding  surfaces  are: 


(3.18) 


o,  =  1.0  w  +  0.6750  6  -0.3400 z 
a  2  =  1.0  4+  1.3500  6  -  0.68002 


(3.19) 


Here  we  define  the  states  to  be  interpreted  as  errors  between  their  actual  and 
desired  values.  With  this  in  mind,  z,  can  be  written  as,  z  -  zd. 

Now  that  the  sliding  surfaces  have  been  calculated,  the  control  laws  are 


determined  from  (2.27): 


(3.20) 


/bv  =  -(0.5420xl04)  w  +  (1.0557xl04)^  +  (0.1354xl04)  6 

-  (0.3097xl04)  r]2 satsgn(a j)  +  (0.9703xl04)  r)2 sats^n{a 2) 

7sv  =  (O^SxlO4)^  -  (1.7829xl04)^  -  (0.2819xl04) 6 

-  (0.3464xl04)  rj 2 satsgn(ox)  -  (1.2169xl04)  rj2 satsgn(a2) 

The  reader  should  note  that  the  same  ry  has  been  used  in  front  of  both  satsgn 
functions  for  simplicity,  although  this  is  not  required. 

3.  Computer  Simulation  with  Pole  Placement  Controller 

Fortran  program  SDVDIVPPTTPSFPOLE.FOR  (see  Appendix  A)  was 
used  to  simulate  the  AUV  in  the  dive  plane  for  all  three  regions  -  hover,  transition, 
and  cruise  -  using  controllers  designed  by  pole  placement.  The  program  assumes 
perfect  state  feedback.  Drag  on  the  AUV  is  calculated  in  the  program  by  the 
trapezoidal  rule.  The  surge  velocity  of  the  AUV  is  controlled  in  the  program  by  the 
speed  controller  developed  by  Lienard  [Ref.  3],  The  simulation  time  step  is 
At  =  0.01  s. 

Fortran  program  DPLOT2.FOR  (see  Appendix  A)  utilizes  the  raw  data 
generated  by  SDVDIVPPTTPSF  POLE.FOR  and  plots  several  graphs  to  visualize 
the  simulation.  DPLOT2.FOR  calls  plotting  subroutines  from  the  software  program 
DISSPLA  (Copyright  1988  by  Computer  Associates  International,  Inc.).  Figures  2 
and  3  were  generated  by  DPLOT2.FOR  and  show  the  results  of  Run  1  at  u  =  0.25 
ft/s  with  <p  =  2.0  and  ry  =  0.2. 

Figures  2  and  3  clearly  show  the  pole  placement  controller  doesn’t  work. 
The  bow  thruster  forces  the  bow  down  while  the  stern  thruster  lifts  the  stern.  This 
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IBV  &  lsv  (AMPERES) 


Figure  2.  Run  1  -  AUV  Response  in  the  Dive  Plane  at  Hover  Speed,  Pole 

Placement  ( u  =  0.25  ft/s,  <f>  =  2.0,  rj2  =  0.2,  Perfect  State  Feedback) 
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U  &  UD  (FT/SEC) 


Figure  3.  Run  1  -  (continued) 
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pitching  scheme  would  work  well  at  higher  speeds  when  the  AUV  has  surge  velocity 
to  reach  depth;  however,  both  thrusters  need  to  work  in  the  same  direction  in  the 
hover  region. 

What  went  wrong?  At  speeds  in  the  hover  region,  we  need  to  minimize 
q  and  6.  This  ensures  the  vehicle  remains  flat  and  the  thrusters  work  in  the  same 
direction  to  force  the  vehicle  down  to  ordered  depth;  however,  pole  placement 
doesn’t  allow  the  designer  to  choose  which  states  to  minimize.  There  are  an  infinite 
number  of  solutions  for  C2T  and  not  all  of  them  may  be  effective.  The  two 
relationships  (3.17)  we  assumed  to  solve  for  C2T  obviously  didn’t  lead  to  the  desired 
control  action  at  hover  speeds;  although,  they  may  work  at  higher  speeds  with  planes. 
A  designer  could  assume  numerous  relationships/values  and  conceivably  never 
stumble  upon  the  correct  combination. 

It’s  obvious  that  simple  pole  placement,  as  formulated  here,  did  not  work 
in  this  situation.  The  next  step  is  to  use  the  LQR  technique  as  discussed  in  Chapter 
II. 

4.  Controller  Design  by  LQR  Technique 

A  Matrix-x  program  was  written  to  assist  in  the  calculation  of  the  sliding 
surfaces  and  control  laws  via  the  LQR  technique  (see  Appendix  B).  This  program 
was  modified  for  all  subsequent  LQR  controller  designs.  The  designer  interactively 
enters  the  diagonal  values  of  the  minimization  matrix  Q.  As  discussed  in  the  previous 
section,  minimizing  q  and  0  should  yield  sliding  surfaces  and  control  laws  that  result 
in  the  correct  control  action  in  the  hovering  region.  To  minimize  these  states,  the 
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values  Qn  =  5,  Q22  =  1000,  £>33  =  500,  and  Q44  =  1  were  selected  after  some 
experimentation.  The  resulting  sliding  surfaces  are: 


a  j  =  1.0  w  -  0.1452  0  +  0.4471  (z-zd) 
a2  =  \.0q  +  0.7074  0  -  0.0007  (z  -zd) 

and  control  laws: 


(3.21) 


7bv  =  -(1.2666x10 3)w  +  (6.8616xl03)<?  -  (0.2037xl03)  0 

-  (0.3097xl04)  q2  satsgn(o  +  (0.9703xl04)  q2 satsgn(o,)  ($22) 
7sv  -  -(1.5299x10 3)w  -  (7.1672xl03)^  +  (1.3030xl03)  0 

-  (0.3464xl04)  q2satsgn(ox)  -  (1.2169xl04)  r/2 satsgn(a^) 

In  addition  to  the  two  zero  poles,  this  controller  has  poles  of  -0.4477  and  -0.7068  on 
the  sliding  surfaces. 

5.  Computer  Simulation  with  LQR  Controller 

Fortran  program  SDVDIVPPTTPSF  LQR.FOR  (see  Appendix  A)  was 
used  to  simulate  the  AUV  in  the  dive  plane  for  all  three  regions  -  hover  transition, 
and  cruise  -  using  controllers  designed  by  the  LQR  technique.  The  program  is  a 
duplicate  of  the  pole  placement  simulation  program  with  the  only  difference  being 
the  controllers.  Fortran  program  DPLOT2.FOR  was  again  used  to  plot  graphs  of  the 
simulation.  Figures  4,  5, 6,  and  7  were  generated  by  DPLOT2.FOR  and  demonstrate 
the  results  at  two  different  speeds  in  the  hover  region.  Figures  4  and  5  are  of  Run 
2  at  u  =  0.00  ft/s,  and  Figures  6  and  7  are  of  Run  3  at  u  =  0.55  ft/s.  Both  runs  were 
executed  with  values  of  <p  -  2.0  and  p2  =  0.2. 
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Figure  4.  Run  2  -  AUV  Response  in  the  Dive  Plane  at  Hover  Speed,  LQR 
(u  =  0.00  ft/s,  <p  =  2.0,  rj1  =  0.2,  Perfect  State  Feedback) 
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Figure  5.  Run  2  -  (continued) 


(u  =  0.55  ft/s,  <p  =  2.0,  77 2  =  0.2,  Perfect  State  Feedback) 
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Figures  4  to  7  conclusively  show  the  LQR  controller  works  satisfactorily. 
The  graphs  of  q  and  8  reveal  the  vehicle  has  a  minimal  amount  of  pitch  angle  as  it 
is  thrusted  downwards.  The  AUV  expeditiously  reaches  its  ordered  depth  with  a 
minor  amount  of  overshoot  by  using  heave  velocity  w  created  by  the  thrusters 
working  together. 

Note  the  simulations  show  the  AUV’s  surge  velocity  u  increasing  when  the 
thrusters  are  energized  due  to  the  unmodeled  effect  of  the  thrusters  on  the  surge 
velocity.  Intuitively,  this  should  not  happen  with  the  real  vehicle. 


D.  CONTROL  OF  THE  AUV  IN  THE  CRUISE  REGION 
1.  State  Space  Equations 

To  review,  in  the  cruise  region,  1.75  <  u  <  6.00  ft/s,  the  thrusters  are  not 
used  and  the  nonlinear  system  is  linearized  at  u  =  3.875  ft/s  to  determine  the  sliding 
surfaces  and  control  laws.  The  resulting  linear  state  space  representation  is: 
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(3.23) 


Again,  the  B  matrix  does  not  need  to  be  transformed. 

2.  Controller  Design  by  Pole  Placement 

It  will  be  interesting  to  investigate  if  an  effective  controller  can  be  designed 
by  pole  placement  at  cruise  speeds.  The  sliding  surfaces  and  control  laws  are 
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designed  via  pole  placement  identically  with  the  procedure  in  the  hover  region.  The 
same  poles  (-0.50,  -0.51)  are  utilized,  and  the  same  two  relationships  (3.17)  are 
assumed.  The  resultant  sliding  surfaces  are: 


a,  =  1.0 w  +  0.5215  6  -0.0329  (z-zd) 
a,  =  1.0  <7  +  1.0429  6- 0.0658  (z-zd) 

which  yields  the  following  control  laws: 


(3.24) 


6bp  =  -2.3407  w  -  7.1582  <jf  -  1.99716 

+  9.2671  T]2 satsgn(o x)  -  18.1220  q2 sats&i(o,)  (3  25) 

6sp  =  -0.5329  w  +  3.2909  q  +  1.83796 

+  1.7528  q 2  safsgn(a1)  +  8.3426  q 2  satsgn(a^) 

3.  Computer  Simulation  with  Pole  Placement  Controller 

Figures  8  and  9  were  generated  by  SDVDIVPPTTPSF_POLE.FCR  and 
DPLOT2.FOR  and  conclusively  prove  the  pole  placement  controller  works  well  in 
the  cruise  region.  The  figures  exhibit  Run  4  at  u  =  3.875  ft/s,  <p  =  1.0,  and 
t)2  =  0.35.  The  AUV  reaches  ordered  depth  quickly  with  negligible  overshoot. 

Pole  placement  is  obviously  not  a  dependable  procedure  for  designing 
MIMO  sliding  mode  controllers  and  will  not  be  used  again  during  this  thesis.  The 
procedure  we  used  to  design  a  unsatisfactory  controller  in  the  hover  region  worked 
well  when  it  was  applied  to  design  a  controller  in  the  cruise  region,  and  even  this  may 
have  been  a  matter  of  luck.  The  only  advantage  of  pole  placement  is  that  it  can 
guarantee  negative  real  poles  on  the  sliding  surfaces,  but  as  we  saw  in  the  hover 
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Figure  8.  Run  4  -  AUV  Response  in  the  Dive  Plane  at  Cruise  Speed,  Pole 

Placement  ( u  =  3.875  ft/s,  <p  =  1.0,  t)2  =  0.35,  Perfect  State  Feedback) 


42 


U  &  UD  (FT/SEC) 


60  100  160 
T  (SEC) 


60  100  160  200 

T  (SEC) 


Q_  « 

cc 


SOLID=U 
DASH-  UD 


O  )  1  1 

0 

'  '  [  '  '  '  '  1  1  '  i T_i 

60  100  160  200 

0 

1  !  |  1  1  1  1  |  1  1  1  1  |  T  f  1  T  | 

60  100  160  200 

T  (SEC) 

T  (SEC) 

43 


region  this  may  not  be  the  most  important  consideration. 

4.  Controller  Design  by  LQR  Technique 

In  the  cruise  region,  the  designer  is  faced  with  a  dilemma  when  using  the 
LQR  technique.  Which  states  should  be  minimized  to  get  the  desired  pitching 
control  action?  Extensive  experimentation  led  to  minimizing  w,  q,  and  0  as  the  best 
solution.  The  diagonal  of  the  LQR  minimization  matrix  Q  was  selected  as  Qn  =  100, 
Q22  =  100,  @33  =  100,  and  @44  =  1.  Matrix-x  was  again  used  to  assist  in  the 
calculations.  The  resulting  equations  are: 


a,  =  1.0  w-  0.0945  6  +  0.0328  (z-zj 
1  V  d'  (3.26) 

o2  =  1.0<?  +  1.3127  0- 0.0945  (z-zd) 

6bp  =  -1.2124  w  -  17.7554g  -  6.3692  0 

+  9.2671  t] 2 satsgn(o^)  -  18.1220  q2  satsgn(o 
6sp  =  -0.6568  w  +  4.4622  q  +  2.31810 

+  1.7528  q2satsgn(al)  +  8.3426  q2satsgn(a^) 

The  two  nonzero  poles  of  this  controller  are  -0.4438  and  -0.9017. 

5.  Computer  Simulation  with  LQR  Controller 

Figures  10, 11,  12,  and  13  show  Runs  5  and  6  for  the  cruise  LQR  controller 
at  1.90  ft/s  and  6.00  ft/s.  The  same  values  of  <p,  rj 2,  and  zd  have  been  used  as  in  Run 
4  for  the  cruise  pole  placement  controller.  Again,  the  controller  works  well;  the 
AUV  reaches  ordered  depth  quickly  with  negligible  overshoot.  As  expected,  the 
speed  of  response  of  the  AUV  increases  with  surge  velocity  due  to  the  larger 


44 


Figure  10.  Run  5  -  AUV  Response  in  the  Dive  Plane  at  Cruise  Speed,  LQR 
(u  =  1.90  ft/s,  <(>  =  1.0,  rf  =  0.35,  Perfect  State  Feedback) 
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Figure  12.  Run  6  -  AUV  Response  in  the  Dive  Plane  at  Cruise  Speed,  LQR 
(u  =  6.00  ft/s,  <p  =  1.0,  Tf  =  0.35,  Perfect  State  Feedback) 


47 


hydrodynamic  forces  on  the  planes  at  higher  speeds.  The  dominance  of  the  stem 
planes  can  be  seen  on  the  graph  of  planes  versus  time  in  Figure  12.  The  stem  planes 
actually  shift  over  to  a  negative  deflection  as  ordered  depth  is  approached  to  reduce 
the  pitch  angle  and  eliminate  overshoot. 

The  heave  velocity  w  plays  an  insignificant  role  in  reaching  zd.  In  fact,  the 
AUV  achieves  a  much  smaller  heave  velocity  in  the  cruise  region  as  compared  to  the 
LQR  controller  in  the  hover  region.  Instead,  the  AUV  uses  pitch  angle  and  surge 
velocity  to  reach  ordered  depth.  In  Figure  13  at  u  =  6.00  ft/s,  we  can  see  the  AUV 
reaches  a  diving  pitch  angle  of  about  -24  degrees. 

E.  CONTROL  OF  THE  AUV  IN  THE  TRANSITION  REGION 
1.  State  Space  Equations 

As  discussed  earlier,  both  planes  and  thrusters  will  be  used  in  the  transition 
region  due  to  their  comparable  effects  in  this  speed  range.  With  all  four  inputs 
utilized,  the  control  distribution  matrix  B  at  u  =  1.250  ft/s  is: 


-0.007959 

-0.017288 

0.000166 

0.000132 

0.001672 

-0.008841 

-0.000047 

0.000042 

0.0 

0.0 
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0.0 
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(3.28) 


Now,  there  are  the  same  number  of  inputs  as  states,  or  in  other  words,  n  =  m!  This 
creates  a  severe  problem.  Utkin’s  method  for  designing  a  MIMO  sliding  mode 
controller  requires  the  Bx  matrix  be  nonsingular  and  of  dimension  mxm;  hence,  Bx 
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must  be  a  4x4  matrix  which  makes  it  equivalent  to  B !  In  this  case,  however,  Bt  is 
obviously  singular  and  noninvertible: 


-0.007959 

-0.017288 

0.000166 

0.000132 

B,  = 

0.001672 

-0.008841 

-0.000047 

0.000042 
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0.0 
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0.0 

0.0 

0.0 
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(3.29) 


The  cause  of  the  problem  can  be  understood  by  examining  the  four 
equations  that  comprise  the  state  space  representation.  There  are  only  two 
equations,  heave  (3.1)  and  pitch  (3.2),  that  contain  the  control  inputs.  The  other  two 
equations,  (3.4)  and  (3.5),  are  kinematic  relations  which  do  not  assist  in  the 
determination  of  the  inputs.  Thus,  there  are  four  unknowns  -  /bv,  7SV,  6bp,  and  6sp  - 
and  only  two  equations.  The  control  inputs  cannot  be  uniquely  determined  and  an 
optimization  scheme  is  needed.  This  was  actually  done  in  the  hover  and  cruise 
regions  by  reducing  the  number  of  inputs  from  four  to  two  based  on  their 
effectiveness,  i.e.,  thrusters  when  hovering  and  planes  when  cruising. 

In  the  transition  region  the  effectiveness  of  the  control  inputs  are 
essentially  equivalent,  and  the  number  of  inputs  can  be  reduced  by  assuming  a 
relationship  between  them.  Here  it  was  assumed: 


^sv  A>v 

^sp  “  -^bp 


(3.30) 


and  the  linear  state  space  representation  at  u  =  1.25  ft/s  becomes: 
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The  LOR  technique  can  now  be  applied  to  design  a  M1MO  sliding  mode 


controller. 


2.  Controller  Design  by  LQR  Technique 

After  some  experimentation,  the  LQR  minimization  matrix  Q  used  in  the 
cruise  region  was  again  used  in  the  transition  region,  with  Qn  =  100,  Q22  —  100, 
<233  =  100,  and  <244  =  L  The  controller  equations  become: 


o.  =  1.0 w  -0.0735  6  +  0.0678 (z-z.) 

1  v  d'  (3.32) 

o,  =  1.0 <7  +  1.0855  6  -  0.0735  (z  -zd) 

6bp  =  5.1  w  -  81.8<7  -  2.3  0 

-  1.6  r) 2 satsgii (a t)  -  93.7  q2 satsgn(o^) 

6  =  -6. 

sp  bp  (3.33) 

7bv  =  - 134.7  w  +  3314.7  q  +  271.2  6 

-  3309.5  tj2 satsgn(a x)  +  2937.0  p 2  satsgri(o7) 

^sv  —  ^bv 

and  the  two  nonzero  poles  are  -0.1614  and  -0.9919. 

3.  Computer  Simulation  with  LQR  Controller 

Figures  14  to  17  show  simulations  of  the  AUV  as  generated  by 
SDVD1VPFTTPSF  LQR.FOR  and  DPLOT2.FOR  at  two  different  speeds  in  the 
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transition  region.  Figures  14  and  15  show  Run  7  at  u  =  0.90  ft/s,  and  Figures  16  and 
17  show  Run  8  at  u  =  1.60  ft/s.  Both  runs  were  executed  with  <p  =  2.0  and  rf  =  0.2. 
The  figures  establish  the  controller  works,  if  not  spectacularly.  This  marginal 
performance  is  caused  by  the  changing  hydrodynamic  forces  on  the  planes  in  the 
transition  region.  At  speeds  above  u  =  1.25  ft/s,  the  planes  begin  to  dominate.  At 
lower  speeds,  the  thrusters  begin  to  dominate.  The  thrusters  work  together  as  they 
did  in  the  hover  region,  while  the  planes  deflect  oppositely  to  create  a  pitch  angle  as 
they  did  in  the  cruise  region. 

a.  Modified  LQR  Controller 

The  above  scheme  works  well  at  the  bow;  the  bow  planes  and  bow 
thruster  always  work  together.  On  the  other  hand,  the  stern  is  a  problem  because 
the  stern  planes  and  stern  thruster  always  fight  each  other.  This  detrimental  effect 
can  be  rectified  by  turning  off  the  stern  thruster  at  high  speeds  (above  1.25  ft/s)  and 
by  centering  the  stern  planes  at  low  speeds  (below  1.25  ft/s). 

Computer  program  SDVDIVPPTTPSFLQR.FOR  was  altered  to 
enact  this  modified  controller  and  Figures  18  to  21  represent  the  results  of  Runs  9 
and  10  using  this  controller.  Note  Figures  18  to  21  are  labeled,  "MODIFIED".  Since 
all  the  set  points  are  identical,  Runs  7  and  8  can  be  easily  compared  to  Runs  9  and 
10.  The  modified  controller  is  clearly  superior,  especially  at  the  slower  speed. 

By  modifying  the  controllers,  the  transition  region  has  in  effect  been 
split  into  two  regions;  in  essence  there  are  a  total  of  four  control  laws.  If  time 
permitted  another  iteration,  a  more  elegant  solution  would  be  use  the  new 
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Figure  14.  Run  7  -  AUV  Response  in  the  Dive  Plane  at  Transition  Speed,  LQR 
( u  =  0.90  ft/s,  <p  =  2.0,  r\2  =  0.2,  Perfect  State  Feedback) 


53 


U  &  UD  (FT/SEC) 


60  100  160 

T  (SEC) 


60  100  160  200 

T  (SEC) 


2  § 
Q.  CO 

CC 


SOLID-U 
DASH-  U0 


—  1  ' 

0 

GO  100  160  200 

T  (SEC) 

1  1 

0 

SO  100  160  200 

T  (SEC) 

54 


IBV  &  Isv  (AMPERES) 


60LID=Z 
DASH-  ZD 

t  }  »  \  !  »  ;  t  t  t  i — pt— r-  ■)  i  ) 

60  100  160  200 

T  (SEC) 


/  SOLID=<5bp 

/  DASH-  <5ep 

1  I  1  1  1  1  I  1  1  1-1  I  1  1  1  ■  I 

60  100  160  200 

T  (SEC) 


$  . 


SOLID=l.v 
DASH—  l|y 

T— | — I  I  I — I— [ — I — I  I  I  |  I  I  I  I  | 

60  100  160  200 

T  (SEC) 


60  100  160  200 
T  (SEC) 


Figure  16.  Run  8  -  AUV  Response  in  the  Dive  Plane  at  Transition  Speed,  LQR 
(u  =  1.60  ft/s,  <p  =  2.0,  q2  =  0.2,  Perfect  State  Feedback) 
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Figure  18.  Run  9  -  Diving  Response  at  Transition  Speed,  MODIFIED  LQR 
(u  =  0.90  ft/s,  <p  =  2.0,  Tf  =  0.2,  Perfect  State  Feedback) 
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(u  =  1.60  ft/s,  <p  =  2.0,  q2  =  0.2,  Perfect  State  Feedback) 
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assumptions  to  redesign  two  separate  controllers  in  the  transition  region,  one  for 
speeds  above  1.25  ft/s  and  one  for  speeds  less  than  1.25  ft/s.  Theoretically  the 
effectiveness  of  the  controllers  will  be  enhanced  if  surge  velocity  is  divided  into 
increasingly  smaller  regions,  each  with  unique  sliding  surfaces  and  control  laws. 
However,  experience  dictates  that  this  scheme  cannot  be  taken  too  far.  The 
computer  code  cannot  be  made  so  large  that  it  is  unable  to  control  the  AUV  in  real 
time  without  unnecessary  computational  delays. 

The  control  of  the  AUV  in  the  transition  region  is  difficult  and  the 
only  consolation  is  the  real  vehicle  will  be  operated  in  this  region  only  for  short 
periods  of  time  as  it  speeds  up  to  cruise  or  slows  to  hover. 

F.  OBSERVERS 
1.  Design 

In  addition  to  the  chattering  problem  discussed  in  Chapter  II,  the 
requirement  of  full  state  feedback  is  a  major  disadvantage  of  the  sliding  mode  control 
technique  because  in  most  cases  one  or  more  of  the  states  cannot  be  directly 
measured.  Up  to  this  point,  it  has  been  assumed  the  controllers  have  perfect  state 
feedback.  In  fact,  the  real  AUV  has  no  sensor  for  measuring  the  heave  velocity  w. 
The  predicament  is  easily  overcome  by  designing  a  reduced  order  (Luenberger) 
observer  to  estimate  the  heave  velocity  w.  The  method  of  designing  a  Luenberger 
observer  is  well  known  and  will  not  be  discussed  in  detail  here;  however,  a  couple  of 
interesting  points  need  to  be  discussed. 
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The  observers  are  designed  starting  with  the  governing  equations  expressed 
in  the  linear  state  space  representation  of  (2.1),  restated  here: 

x  =  Ax  +  Bu  (3.34) 

The  A  matrix  is  the  same  for  all  three  speed  regions  if  it  is  expressed  as  a  function 
of  surge  velocity  u: 


auu 

anu 

fl13 

0.0 

a2lu 

o22u 

^23 

0.0 

0.0 

fl32 

0.0 

0.0 

*4, 

0.0 

V 

0.0 

(3.35) 


Thus,  this  A  matrix  can  be  used  for  all  three  regions;  however,  this  cannot 
be  done  for  the  B  matrix  because  the  inputs  u  are  different  in  each  region.  If  B  is 
expressed  as  a  function  of  surge  velocity  u  for  each  region,  they  can  be  written  as: 


•  Hover  region: 


b  n  (0.25  -  0.0083m) 
b2X  (0.25  -0.0083m) 
0.0 
0.0 


bl2(0.25  -  0.0083k) 
b22  (0.25  -  0.0083  m) 
0.0 
0.0 


(3.36) 


62 


•  Transition  region: 


buu2  bn (0.25  -  0.0083 u) 
b21u2  b22  (0.25  -0.0083m) 


0.0 

0.0 


•  Cruise  region: 


B  = 


0.0 
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bxxU2 

bnu 

b2lu- 

b22u 
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0.0 

(3.37) 


(3.38) 


Expressing  B  in  general  terms: 


*n 

*12 

*21 

*22 

0.0 

0.0 

0.0 

0.0 

(3.39) 


Then  the  observer  equations  can  also  be  expressed  in  general  for  all  three  observers: 


w  =  Lxq  +  L,0  +  L3(2-2d)  +  2 
2  =  F2  +  Gxq  +  G,0  +  G3(z-2d)  +  Hxux  +  //,«, 


F  =  anu  -  Lxalxu  -  L2aAX 


(3.40) 

(3.41) 

(3.42) 


Gx=anu-Lxa22u-L2a22+FLV  G2  =  a13-L1a23-L3o43u+FL2,  G3  =  FL3  (3.43) 
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H\  =  B\\  ~L\B2\  >  H2  =  B12  ~L\B22 

(3.44) 

a,,u  -  S, 

Lx  =  11  1  ,  L,  =  0.0  ,  L3  =  Lx 

a2lU+a4\ 

(3.45) 

u{  s  input  1  ,  u2  s  input  2 

(3.46) 

5,  =  observer  pole 

(3.47) 

2.  Simulation  with  Observers 

The  LQR  simulation  program  was  rewritten  with  observers  for  the  heave 
velocity  w.  The  new  program  is  called  SDVDIVPPTTOBSLQR.FOR  and  can  be 
found  in  Appendix  C.  The  three  observers  are  nested  in  subroutines  at  the  end  of 
the  program.  DISSPLA  plotting  program  DPLOT4.FOR  (see  Appendix  A)  was  used 
to  generate  graphs  of  simulation  runs. 

To  allow  the  observers  to  settle  out  after  the  start  of  the  simulation,  a  10 
second  delay  was  programmed  into  all  the  observers  during  which  the  estimated  value 
of  w  was  set  to  zero.  The  pole  of  each  observer  was  set  to  a  value  twice  as  fast  as 
the  slowest  sliding  surface  pole  in  the  respective  speed  region. 

Figures  22  to  27  show  simulation  Runs  11  to  13  that  were  generated  by  the 
programs.  Each  run  was  conducted  in  a  different  controller  speed  region.  The 
figures  reveal  that  the  observers  work  well.  Vehicle  control  is  not  compromised  by 
using  an  estimated  value  for  the  heave  velocity. 

It  is  imperative  that  an  observer  for  the  heave  velocity  is  utilized  at  slower 
speeds  because  w  becomes  the  predominant  value  used  by  the  controller.  In  fact,  it 
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(u  =  0.00  ft/s,  <p  =  2.0,  rf  =  0.2,  w  Observed) 
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-25 


Figure  24.  Run  12  -  AUV  Response  in  th°  Dive  Plane  at  Cruise  Speed,  LOR 
( u  -  6.00  ft/s,  <p  =  1.0,  /72  =  0.35,  w  Observed) 
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Figure  27.  Run  13  -  (continued) 
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was  found  that  an  observer  was  not  needed  in  the  cruise  region  because  w  is 
inconsequential  at  high  speeds. 

G.  TRANSITION  IN/OUT  OF  HOVER 

One  of  the  most  difficult  maneuvers  for  an  AUV  is  the  transition  into  or  out 
of  hover.  This  is  because  control  of  the  vehicle  is  passing  between  controllers  and 
observers.  Program  SDVDIVPPTTOBS  LQR.FOR  was  modified  for  differing  initial 
conditions  to  explore  this  maneuver.  Figures  28  and  29  show  Run  14  in  which  the 
AUV  transitions  into  hover  while  conducting  a  depth  change.  This  is  a  radical 
maneuver.  Figures  30  and  31  show  Run  15  in  which  the  AUV  transitions  out  of 
hover  while  conducting  a  depth  change. 

The  figures  exhibit  the  resilience  of  the  controllers  and  observers.  The  radical 
maneuvers  create  disturbances  on  the  vehicle,  but  they  are  overcome  by  the  control 
package  and  the  vehicle  settles  out.  In  Run  14,  there  is  some  oscillation  of  the 
vehicle  around  0  degree  of  pitch  as  it  is  thrusted  to  ordered  depth;  however,  the 
oscillation  is  only  about  ±  4  degrees  and  is  acceptable. 

H.  CONCLUDING  REMARKS 

A  controller  package  has  been  designed  for  the  AUV  that  effectively  controls 
the  vehicle  throughout  its  entire  operational  speed  range.  The  controller  package  has 
been  optimized  by  dividing  up  the  surge  velocity  into  three  different  regions  and 
designing  a  separate  controller  and  observer  for  each  region.  This  strategy  is 
necessary  because  the  hydrodynamic  forces  on  the  planes  change  with  surge  velocity. 
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Figure  28.  Run  14  -  AUV  Response  While  Transitioning  to  Hover,  LQR 
( <p  -  2.0,  ry  =  0.2,  w  Observed) 
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Figure  29.  Run  14  -  (continued) 
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Figure  30.  Run  15  -  AUV  Response  While  Transitioning  Out  of  Hover,  LQR 
( <p  =  2.0,  0.2,  w  Observed) 
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Obviously  to  enact  a  MIMO  sliding  mode  controller  for  the  real  NPS  AUV,  it 
will  be  necessary  to  verify  all  hydrodynamic  coefficients.  In  addition,  Kalman  filters 
will  have  to  be  used  on  the  real  vehicle  to  smooth  the  measurements  of  the  states. 
This  due  to  noise. 
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IV.  LINE-OF-SIGHT  GUIDANCE  OF  THE  AUV  USING  MIMO  SLIDING 
MODE  CONTROL 

A.  INTRODUCTION 

Control  of  the  AUV  in  the  horizontal  plane  will  now  be  analyzed.  Steering 
control  of  the  AUV  is  essentially  analogous  to  depth  control,  with  minor  excursions. 
The  first  difference  is  the  predominance  of  disturbances  in  the  horizontal  plane, 
especially  ones  of  a  steady-state  nature.  Transient  disturbances  are  easily  handled 
by  the  inherent  robustness  of  the  sliding  mode  control  design. 

In  the  dive  plane,  environmental  steady-state  disturbances  are  uncommon; 
ocean  currents  are  mamly  horizontal.  Or  course,  transient  disturbances  due  to  wave 
action  occur  near  the  surface.  However,  the  effect  of  ocean  currents  on  the  AUV 
requires  steady-state  disturbances  to  be  accounted  for  in  steering  controller  design. 

Another  difference  of  steering  control  is  the  requirement  to  adhere  to  a 
navigation  scheme.  Two  common  methods  of  navigation  are  line-of-sight  (LOS)  and 
cross-track-error  (CTE)  guidance.  This  chapter  deals  with  LOS  guidance,  while  the 
latter  is  discussed  in  Chapter  V.  LOS  guidance  of  the  NPS  AUV  was  first 
investigated  by  Lienard  [Ref.  3]  for  SISO  systems. 

For  two  reasons,  this  thesis  will  investigate  steering  control  at  higher  speeds  by 
the  use  of  rudders  only.  First,  time  and  space  requirements  for  this  thesis  don’t 
permit  analyzing  the  horizontal  plane  in  the  same  manner  as  the  dive  plane  -  at  three 
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different  speed  ranges.  Second,  and  more  importantly,  once  MIMO  steering  with 
rudders  has  proven  effective  for  guidance  control,  it  would  be  unnecessarily  repetitive 
to  repeat  the  analysis  with  horizontal  tunnel  thrusters.  Obviously,  the  three  tier 
control  hierarchy  used  successfully  for  diving  can  be  extended  to  steering. 

B.  NONLINEAR  GOVERNING  EQUATIONS  IN  THE  HORIZONTAL  PLANE 

1.  Addition  of  Bow  Rudder  to  SDV  Model 

As  discussed  in  Chapter  III,  a  bow  rudder  needs  to  be  added  to  the  SDV 
model  of  [Ref.  7]  to  approximate  the  geometry  of  the  NPS  AUV.  The  assumption 
was  made  that  the  bow  rudder  would  have  the  same  effect  on  the  vehicle  as  the 
"real''  stern  rudder.  Hence,  the  magnitude  of  the  hydrodynamic  coefficients  would 
be  equal  for  the  two  rudders;  although,  in  some  cases  they  would  be  of  opposite  sign: 

y6  =  2.730xl0-2  V6  =  2.730xl0-2 

°br  °sr 

=  1.290x1  O'2  M  =  -1.290X10-2 

°br  °sr 

X1..  =  8.180X10-4  X/r6  =  -8.180X10-4  (4.1) 

robr  rosr 

X/vS  =  1.730xl0"3  X,v6  =  1.730xl0-3 

vobr  vosr 

X*c  c  -  -l.OlOxlO2  X/6  J  =  -l.OlOxlO-2 

°br°br  °sr°sr 

2.  Assumptions 

For  control  of  the  vehicle  in  the  horizontal  plane,  the  following 
assumptions  have  been  made: 
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•  Vehicle  motion  is  confined  to  the  horizontal  plane: 


{pi  Pi  (b  4i  Qi  <P,  w,  6br, 


^bv’  L 


0) 


•  The  AUV  is  neutrally  buoyant. 

•  The  CB  and  CG  are  on  the  vehicle’s  centerline. 

•  The  AUV  is  restricted  to  speeds  in  which  the  rudders  are  effective. 

•  The  rudders  have  a  restricted  range  of  motion  identical  to  the  planes. 

•  Surge  velocity  can  be  assumed  constant  in  the  yaw  and  pitch  equations. 

•  Rudders  are  infinitely  responsive,  i.e.,  no  lag  in  reaction  time.  (This  is  a  valid 
assumption  because  steering  gear  dynamics  are  much  faster  than  the  dynamics 
of  a  small  turning  vehicle.) 

3.  Resulting  Equations 

After  all  modifications  and  assumptions,  the  nonlinear  governing  equations 
in  the  horizontal  plane  from  [Ref.  7]  become: 


•  Lateral  (Sway)  equation  of  motion 


m(v  +  ur  +  xgf  -  ygr)  =  |LVtr 


+  — L3(V/vv  +  VTur) 


(4.2) 


lC-[Yvuv  ,  u-ribr6b'  -  VJ.6J] 


p  rbow  CDyh(x)(v  +  xr): 
2  J  stern 


dx 
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•  Yaw  equation  of  motion 


I  r  +  mx  (v  +  ur)  +  my  (vr  -  u)  = 

^V.r 

2  f 

+  ^L4(<  i>  +  N'mk) 

2  (4.3) 

+  +  + 

_  pnbow  CDyh(x)(v  *xr)3^ 

2  J  «ern  l/cf(x) 


•  Surge  equation  of  motion 

m(u  -  vr  -  xr  -  ygr)  = 

—LSx'r 

2 

+  +  ^trvr  +  «r(Y;6br6br  +  KeJJ] 

(4.4) 

+  +  “K4«br<5br  +  ^v6$6sr)] 

*  ^2“-X=p 

•  Kinematic  definition 


(4.5) 
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•  Cross  flow  velocity  equation 


U"<*)  =  k  + 


(4.6) 


•  Inertial  position  rates 


x  =  u  costy  -  v  sim|i 
y  =  u  sintjf  +  v  cosi|r 


(4.7) 


The  propulsion  equations  (3.6)  to  (3.10)  still  apply  in  the  horizontal  plane. 

C.  LINEAR  STATE  SPACE  REPRESENTATION 

The  yaw  and  pitch  equations  are  linearized  at  u  =  3.0  ft/s  to  obtain  the 
equations  of  motion  in  the  linear  state  space  representation  of  (2.1)  for  control  law 
design.  As  in  the  dive  plane,  a  Fortran  program  was  utilized  to  assist  in  the  solution 
of  the  yaw  and  sway  equations  (see  Appendix  A).  With  the  inclusion  of  the  linear 
kinematic  definition  (4.5),  the  state  space  representation  is  complete: 


V 

-0.1265  -1.0547  0.0 

V 

0.1168  0.1036 

r 

= 

-0.0084  -0.2952  0.0 

r 

+ 

0.0398  -0.0382 

k 

0.0  1.0  0.0 

0.0  0.0 

(4.8) 


As  before,  (4.8)  is  partitioned  into  the  form  of  (2.21)  and  (2.22)  for  the  application 
of  Utkin’s  MIMO  sliding  mode  control  design  technique: 


-0.1265  -1.0547 

A,,  = 

0.0 

-0.0084  -0.2952j 

12 

0.0 
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An  =  [0.0  1.0] 


a22  =  [0.0] 


B . 


0.1168  0.1036 
0.0398  -0.0382 


-  [v  $  « w 


u 


6h  <5 

br  sr 


(4.9) 


D.  CONTROLLER  DESIGN  BY  LQR  TECHNIQUE 

Mairix-x  was  again  utilised  for  calculations  by  the  LQR  design  technique.  After 
experimenting  with  the  diagonal  of  the  LQR  minimization  matrix  Q,  it  was  found  that 
satisfactory  results  were  obtained  with  Qn  =  4.0,  Q22  —  4.0,  and  Q33  =  1.0.  The 
resulting  sliding  surfaces  are: 


a  j  =  l.Ov 

a,  =  l.Or  +  0.5(i|i  -  ) 


(4.10) 


The  third  state  is  expressed  explicitly  as  the  difference  between  the  actual  and  desired 
values.  Hence,  ijrd  is  the  Euler  yaw  angle  in  the  direction  of  the  desired  navigational 
reference  point  and  is  measured  clockwise  from  due  north  which  is  defined  as  i|r  =  0°. 
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With  (4.10),  the  control  laws  can  be  determined: 


6br  =  0.6642  v  +  2.2219r  -  4.4499  p 2 satsg)i(ox) 

-  12.0713  r)2 satsgn(o ,) 
6sr  =  0.4725  v  +  7.6752r  -  4.6361  q 2 satsgnfa^ 

+  13.6037  Tj 2  satsgn(a^) 

The  resultant  sliding  surface  pole  is  -0.50. 


(4.11) 


E.  LINE-OF-SIGHT  GUIDANCE  SCHEME 

1.  Hit  Criterion 

The  steering  controller  just  designed,  (4.10)  and  (4.11),  must  be  able  to 
navigate  the  AUV  through  a  series  of  way-points  in  the  horizontal  plane.  For  the 
LOS  controller,  the  criterion  for  a  "hit"  of  a  way-point  has  been  determined  to  be 
when  the  AUV  is  within  half  a  ship  length  or  8.7125  ft,  i.e.,  the  AUV  has  reached  a 
way-point  if  it  is  within  a  circle  of  radius  0.5L  centered  on  the  way-point.  In  practice, 
the  hit  criterion  used  would  depend  upon  the  AUV’s  mission. 

2.  Shortest  Turn 

For  efficiency  the  AUV  should  always  turn  through  the  shortest  angle  to 
achieve  the  desired  heading.  This  is  accomplished  in  the  simulation  by  the  excerpt 
ot  Fortran  code  in  Figure  32.  It  was  decided  that  angles  within  the  simulation 
program  would  kept  between  -rr  and  n  by  using  the  intrinsic  Fortran  function 
ATAN2.  This  results  in  a  discontinuity  at  due  south,  vice  due  north 
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c 

C  CALCULATE  the  quickest  route  psid 

c 

IF((PSI  .GE.  0.0  AND.  PSI  .LE.  PIE)  .AND.  (PSID  ,GE.  -PIE  .AND.  PSID  .LE. 
PSI-PIE))  THEN 

PSID  =  PSID  +  2.0*PIE 

ELSEIF((PSI  .GE.  -PIE  .AND.  PSI  .LT.  0.0)  .AND.  (PSID  .GT.  PSI+PIE  .AND.  PSID 
.LE.  PIE))  THEN 

PSID  =  PSID  -  2.0*PIE 

ENDIF 


Figure  32.  Fortran  Code  for  Calculating  the  Shortest  Turn 


v}rd  is  the  only  exception  to  the  rule  of  angles  from  -nr  to  n.  The  Fortran 
code  of  Figure  32  allows  i|rd  to  be  less  than  -tt  or  greater  than  n  in  situations  when 
i|r  and  i|rd  are  on  opposite  sides  of  due  south  and  the  quickest  route  is  across  the 
discontinuity.  This  allows  the  AUV  to  "chase"  the  desired  heading  across  the 
discontinuity.  This  is  very  similar  to  the  method  used  by  Lienard  [Ref.  4], 


F.  SIMULATION 


Programs  SDVLOSRR300PSF_LQR.FOR  (see  Appendix  A)  and 
PLOT7A.FOR  (see  Appendix  A)  were  written  to  simulate  the  AUV  in  the  horizontal 
plane  with  perfect  state  feedback  (PSF).  Figures  33  and  34  show  Run  16  conducted 
at  an  ordered  speed  of  u  =  6.0  ft/s,  <p  =  2.0,  and  ry  =  0.5.  Lienard’s  speed 
controller  [Ref.  3]  was  again  employed.  The  speed  controller’s  saturation  function 
was  kept  at  <p  =  1  6  for  all  runs  because  it  was  found  to  give  the  best  results. 
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Figure  33.  Run  16  -  AUV  Steering  Response  in  the  Horizontal  Plane,  LQR 

(:/  =  6.0  ft/s,  <p  =  2.0,  ?72  =  0.5,  Perfect  State  Feedback,  No  Current) 
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Figure  34.  Run  16  -  (continued) 


Initial  conditions  of  the  simulation  at  /  =  0  are  the  vehicle  headed  north 
(♦  =  °°)  at  ordered  speed  i/d  with  rudders  centered.  The  positive  x-axis  is  due  north 
and  the  positive  y-axis  is  due  east.  The  small  circles  on  the  global  plot  of  Figure  33 
mark  the  location  of  the  way-points.  (Nothing  should  be  inferred  from  the  circles’ 
size.)  The  global  plot  shows  the  vehicle  smartly  hitting  the  desired  way-points. 

The  graph  of  rudder  action  deserves  close  scrutinization.  Although  the  two 
rudders  have  the  same  effect  on  the  vehicle,  i.e.,  the  same  hydrodynamic  coefficients, 
the  control  package  does  not  order  symmetrical  action  of  the  rudders.  The  rudders 
initially  deflect  in  opposite  directions  as  expected,  but  the  stern  rudder  returns  to  zero 
quicker.  In  fact,  the  stern  rudder  continues  past  center  and  deflects  a  small  amount 
in  the  same  direction  as  the  bow  rudder.  Thus,  the  stern  rudder  prevents  overshoot. 
This  action  was  not  consciously  designed  into  the  controller  and  could  not  have  been 
predicted.  Each  rudder  operates  independently  of  the  other.  This  is  a  result  of 
Utkin’s  MIMO  sliding  mode  method. 

G.  STEADY-STATE  DISTURBANCE  COMPENSATION 
1.  Theory 

Figures  35  and  36  show  the  results  of  Run  17  with  a  steady-state  current 
present.  The  current  is  expressed  as  absolute  velocity  components  in  the  inertial 
global  frame,  uco  =  2.0  ft/s  and  vco  =  2.0  ft/s.  Thus,  the  total  current  has  velocity  of 
2.828  ft/s  directed  towards  the  north-east,  i.e.,  t|f  =  45°.  This  is  a  significant  current 
of  almost  50%  of  the  AUV’s  maximum  speed. 
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6Br  &  68r  (RADIANS)  X-AXIS  (FT) 
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Figure  35.  Run  17  -  AUV  Steering  Response,  LQR  (u  =  6.0  ft/s,  <p  -  2.0, 

n2  =  0.5,  PSF,  uco  =  vco  =  2.0  ft/s,  No  Disturbance  Compensation) 


88 


CM 


*"1  |  '  '  1  1  |  I  i  I  i  |  r  i  r 1  |  i  i  i  i  | 

100  200  300  400  600 


T  (SEC) 


T  (SEC) 


”1  ]  »  »  r  i  i  »  i  i  j  !  1-7  r  I  I  I  I  I  | 

100  200  300  400  500 


T  (SEC) 


The  global  plot  on  Figure  35  exemplifies  ♦he  problem  of  uncompensated 
disturbances.  The  vehicle  hits  the  way-points,  but  does  so  via  a  circuitous  route. 
Also,  in  contrast  to  common  sense  the  rudder  angles  are  initially  small  and  utilize 
their  full  strength  only  during  the  final  stage  of  the  approach  to  the  way-point.  Let’s 
determine  the  nature  of  the  problem  and  investigate  a  way  to  compensate. 

First,  the  linear  state  space  system  of  (4.8)  can  be  expressed  in  general 

terms: 


V 

=  anv 

+  Jl2r  +  . 

bll5br  +  ' 

b,2*„ 

r 

=  anv 

+  a22r  + 

fc215br  + 

b22' 

<!'  =  r 


(4.12) 


In  a  like  manner,  the  controller  equations  expressed  in  general  are: 


o,  =  suv  +  si2r  +  5,3(i|;  -  rj/d ) 

a,  =  5,,v  +  s22r  +  5,3(i|»  -  ijjd) 

6br  =  knv  +  k\2r  +  ki3salsSn(°i)  +  kl4satsgn{o2) 

6sr  =  k2iv  +  k22r  ■  k23satw(°i)  +  k24satsgn(o2) 


(4.13) 


(4.14) 


where  the  r\z  terms  have  been  incorporated  within  the  gains  kiv  ku,  k23,  and  k2A  in 
(4.14).  With  current  present,  the  inertial  position  rates  (4.7)  become: 


x  =  uco  +  u  costy  v  sinijf 
y  =  vco  +  u  simji  +  v  costy 


(4.15) 
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Equation  (4.15)  can  also  be  expressed  in  the  local  reference  frame  formed  by  the  leg 
of  two  consecutive  way-points: 


x*  =  uc  +  u  cosijr'  -  v  sinty' 
/  =  v'  +  u  simjF  +  v  cosijF 


(4.16) 


where  and  vc  are  the  current  components  with  respect  to  the  local  leg.  and  ijr '  is 
the  heading  angle  with  respect  to  the  local  leg: 


V  =  (*  -  «) 


a  =  arctan 


y2  -y  i 


(4.17) 

(4.18) 


The  variable  a  in  (4.17)  and  (4.18)  is  the  angle  the  local  leg  makes  with  the  x-axis 
which  is  due  north  or  ijr  =  0°.  Positive  a  is  defined  as  clockwise.  The  points  (jtj,y,) 
and  ( x2 ,  y2)  are  the  coordinates  of  the  two  way-points  that  define  the  local  leg. 
Finally,  the  local  components  of  current  are  related  to  *he  global  components  by: 


u  -  v  sina  +  u  cosa 

C  CO  CO 

v  =  v  cos  a  -  u  sina 

C  CO  CO 


(4.19) 
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When  the  AUV  is  at  a  steady-state  condition,  (4.12)  becomes: 


11 v 

+  al2r 

<5br 

*  VA, 

o 

II 

21  V 

+  a22r 

+  b21 

5br 

*  bn6;, 

=  0 

ij;  =  r  =  0 


(4.20) 


Assuming  zero  deflection  of  the  rudders  at  steady-state  even  in  the  presence  of  a 
current,  (4.20)  results  in: 


v  =  r  =  6br  =  6sr  =  0 
and  (4.21)  then  leads  to  the  conclusion  that: 


a,  =  0 
a,  =  0 


|  -  =  ♦i  “*  =  ll»d 


(4.21) 


(4.22) 


Hence,  the  AUV  achieves  desired  heading  even  in  the  presence  of  a  current!  In  that 
case,  however,  f  is  nonzero  and  y1  increases  linearly  with  time.  The  result  is  the 
AUV  is  continuously  pushed  laterally  away  from  the  leg  by  the  vc  component  of 
current. 

What  is  the  steady-state  value  of  i|ij  ?  Through  the  LOS  guidance  scheme, 
the  desired  heading  becomes  a  function  of  the  instantaneous  ship  position  and  the 
next  desired  way-point.  The  local  desired  heading  i|;d  at  steady-state  can  be 
computed  from  the  kinematics: 


92 


x'  =  Uc  +  u  cos«J/d 
y1  =  vc  +  u  simjfj 


and  from  the  LOS  requirement: 


/  y 

tamj»d  =  — 
x 


Using  (4.23)  and  (4.24),  we  can  see  that: 

,  ,  /  VC 

tamjfd  =  — 


(4.23) 


(4-24) 


(4-25) 


Thus,  the  LOS  guidance  law  eventually  aligns  the  AUV  with  the  current  when  it 
approaches  a  way-point!  This  action  can  be  seen  in  the  global  plot  of  Figure  35. 

The  requirement  /  =  0  can  be  achieved  by  modifying  the  sliding  surfaces 


to: 


o,  =  snv  +  snr  +  s130 |r  -  ijid)  +  s^arcsin 
o 2  =  521v  +  s22r  +  523(i}r  -  i|fd)  +  s23arcsin 


(  \ 
v 


(4.26) 


/  N 
V 

c 

U 


Then  at  steady-state  (4.22)  (a,  =  o2  =  0)  and  hence  (4.21)  are  achieved,  but  there 


is  a  heading  error  at  steady-state  of: 


terror  =  ^crror  -  -  arCSin 

but  in  this  case  the  desired  result  of  /  =  0  is  achieved.  To  put  it  in  simple  terms, 
the  AUV  overcomes  the  local  lateral  component  of  current  by  keeping  its  bow 
headed  into  the  current  an  amount  ijrerror  in  (4.27). 

The  sliding  surfaces  for  the  AUV’s  LOS  controller  can  now  be  explicitly 

expressed: 


u 


(4.27) 


Oj  =  1.0  v 


fvl 


a,  =  1.0  r  +  0.5  (4r  -  tj»d)  +  0.5  arcsin 


(4.28) 


The  reader  should  note  that  we  could  have  assumed  a  zero  local  heading 
error  (i|//  =  0)  at  steady-state,  vice  the  assumptions  of  (4.21).  This  would  lead  to 
nonzero  rudder  deflections  at  steady-state.  We’ll  explore  this  different  approach  in 
Chapter  V  for  the  CTE  guidance  steering  controller. 

2.  AUV  Simulation  with  Steady-State  Disturbance  Compensation 

Fortran  program  SDVLOSRR300PSF  LQR.FOR  was  modified  to  enact 
the  disturbance  compensation  of  (4.28).  Figures  37  and  38  are  for  Run  18  with  the 
modified  sliding  surfaces.  The  only  difference  between  Run  18  and  Run  17  is 
disturbance  compensation,  so  they  can  be  easily  compared.  Disturbance 
compensation  clearly  corrects  for  the  presence  of  the  current.  The  AUV  overcomes 
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Figure  37.  Run  18  -  AUV  Steering  Response,  LQR  ( u  =  6.0  ft/s,  <p  =  2.0, 
t)2  -  0.5,  PSF,  uco  =  vco  =  2.0  ft/s,  Disturbance  Compensation) 
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Figure  38.  Run  18  -  (continued) 


the  current  and  hits  the  designated  way-points.  It  should  be  emphasized  that  in  all 
simulations  where  current  is  present,  the  full  strength  of  the  current  is  instantly 
applied  at  t  =  0,  thus  creating  a  severe  start-up  test  for  the  controller. 

Unfortunately,  one  difficulty  remains.  Ocean  current  is  not  directly 
measurable  by  the  AUV.  Additionally,  at  the  time  of  this  thesis  it  was  not  clear  if 
the  NPS  AUV  would  be  able  to  measure  sway  velocity  v.  Assuming  the  worst,  v  will 
also  be  unmeasured.  As  in  the  diving  situation,  observers  must  be  designed  for  state 
estimation. 

H.  OBSERVERS 
1.  Design 

A  reduced  order  observer  can  be  designed  based  on  x,  y,  t|r,  and  r 
measurements  to  estimate  the  sway  velocity  v  and  the  local  current  components  uc 
and  vc.  The  current  component  uc  does  not  need  to  be  observed  because  the  control 
package  does  not  use  it,  but  it  will  be  included  in  the  output  of  the  reduced  order 
observer  as  an  exercise. 

The  procedure  is  the  same  as  in  the  diving  controller;  however,  to  start  the 
design  process  the  state  vector  is  augmented.  The  linear  state  space  representation 
becomes: 
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v  =  auuv  4  anur  +  bnu26br  +  buu2Ssi 
r  =  a2luv  +  a22ur  +  b2lu26br  +  b22u26si 

\jf7  =  r 

y7  =  vc  +  v  +  u  tJ/;  (4.29) 

x7  =  u  +  u  =  £/ 

c  c 

vc  =  0 
Uc-0 

The  reduced  order  observer  equations  become: 

0  =  +  Lnr 

K  =  ^2  +  L23>/ 

=  £3  +  L^x7  -  u  cosijr7 


(4.30) 

(4.31) 

(4.32) 


^1  -  +  (flj,M  ^12^22^  +  +  (^li  ~  ^21^12  ^ 


br  (4.33) 

+  (fc12  -622L12)M2<5sr 


22  =  S2Zj  +  S2^2  -  L23m  sinf7  +  S2L23/  +  S2Ll2r 


(4.34) 


^3  "  ^3^3  +  ‘^3^'34*/ 


(4.35) 


^12  " 


fl2l“ 


^23  =  ^2 


^34  = 


(4.36) 
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5 r  S,,  53  =  observer  poles 


(4.37) 


The  observer  poles  were  selected  at -1.0,  -1.1,  and  -1.2.  The  estimated  values  dc  andfl. 
are  updated  every  time  a  new  straight  line  segment  is  encountered.  The  reduces  the 
transients  in  the  observer  during  transition  between  one  way-point  to  the  next. 

2.  Simulation  with  Observers 

Programs  SDVLOSRR300OBSLQR.FOR  (see  Appendix  D)  and 
PLOT8.FOR  (see  Appendix  A)  simulate  the  AUV  with  disturbance  compensation 
and  observers.  Figures  39  and  40  show  Run  19  which  is  a  duplicate  of  the  previous 
steering  simulation  runs  but  with  observers.  The  observers  do  a  very  good  job  of 
estimating  the  unknown  states,  and  vehicle  control  is  not  degraded  by  using  these 
estimates. 

All  previous  steering  simulations  runs  (Runs  16-19)  have  been  executed 
with  a  simulation  time  step  of  At  =  0.01  s.  The  control  packages  were  also  updated 
every  0.01  s.  In  reality,  the  AUV’s  controller  may  not  be  updated  this  often.  To 
investigate  what  may  happen,  program  SD  VLOSRR300OBS_LQR.FOR  was  modified 
so  the  control  package  was  only  updated  ever  0.1  s  and  Figures  41  and  42  show  the 
results.  The  only  effect  on  the  vehicle  is  a  slight  degradation  of  the  estimates  of  the 
unknown  states.  This  degradation  is  not  significant  enough  to  jeopardize  vehicle 
control  even  slightly. 


<$En  &  6sr  (RADIANS)  X-AXIS  (FT) 
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Figure  39.  Run  19  -  AUV  Steering  Response,  LQR  (u  =  6.0  ft/s,  <p  =  2.0,  q2  - 
0.5,  Observers,  uco  =  vco  =  2.0  ft/s,  Disturbance  Compensation) 
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Figure  41.  Run  20  -  AUV  Steering  Response,  LQR  ( u  =  6.0  ft/s,  <p  =  2.0,  q2  = 
0.5,  Observers,  «co  =  vco  =  2.0  ft/s,  Disturbance  Compensation) 
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Figure  42.  Run  20  -  (continued) 
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I.  CONCLUDING  REMARKS 


A  MIMO  sliding  mode  controller  has  been  designed  for  the  AUV  which  utilized 
a  line-of-sight  guidance  scheme.  The  controller  proved  to  be  very  effective. 
Disturbance  compensation  was  achieved  by  means  of  a  feedforward  term  in  the 
sliding  surface  equations.  Disturbance  estimation  was  possible  by  using  a  standard 
Leunberger  observer.  The  resulting  scheme  demonstrated  excellent  path  keeping 
characteristics  in  the  presence  of  strong  lateral  currents  wit.  out  any  compromise  in 
stability  and  robustness  properties. 

Line-of-sight  guidance  is  a  good  navigation  scheme  for  open  ocean  cruising. 
However,  LOS  may  not  provide  adequate  guidance  in  restricted  waters  because  it 
allows  the  vehicle  to  overshoot  way-points.  This  problem  leads  us  to  explore  cross¬ 
track-error  guidance  in  Chapter  V. 
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V.  CROSS-TRACK-ERROR  GUIDANCE  OF  THE  AUV  USING  MIMO  SLIDING 
MODE  CONTROL 

A.  INTRODUCTION 

Analysis  of  the  simulation  runs  in  Chapter  IV  show  that  the  AUV  with  LOS 
guidance  is  unable  to  follow  a  perfectly  straight  path  in  the  presence  of  a  steady-state 
disturbance,  even  with  compensation.  This  is  adequate  for  open  ocean  cruising; 
however,  LOS  guidance  will  not  fulfill  the  tolerance  requirements  of  operations  in 
restricted  waters  unless  many  closely  spaced  way-points  are  programmed  into  the 
AUV’s  mission  planning  computer.  Cross-Track-Error  (CTE)  guidance  is  an 
alternate  scheme  for  steering  control  that  will  enable  the  AUV  to  follow  a  straight 
path. 


B.  CROSS-TRACK-ERROR  GUIDANCE  SCHEME 

All  the  tools  we  need  to  design  a  CTE  guidance  steering  controller  were 
introduced  in  Chapter  IV.  The  governing  equations,  (4.2)  to  (4.7),  still  apply.  Also 
valid  are  the  equations  expressed  in  the  local  reference  frame,  (4.16)  to  (4.19).  When 
the  AUV  reaches  a  new  way-point,  that  way-point  becomes  tl.e  new  origin  of  a  local 
coordinate  system  formed  with  the  next  ordered  way-point.  Obviously  this  scheme 
requires  the  augmentation  of  the  state  vector  used  for  LOS  guidance  with  the  cross¬ 
track-error  variable,  yL  The  new  linear  state  space  system  used  for  controller  design 
can  be  expressed  in  general  terms  without  disturbances  as: 
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(5.1) 


11 v 

+  anr  + 

«br  *■ 

*12 

21  V 

+  a22r  + 

*21 

*br  + 

*22 

iji '  =  r 

/  =  v  +  a43V 


The  constants  in  (5.1)  have  been  determined  by  linearization  at  a  particular  surge 
velocity,  u.  As  in  the  previous  chapter,  a  surge  velocity  of  u  =  3.00  ft/s  has  been 
chosen  to  produce  the  linearization  (5.1). 

C.  CONTROLLER  DESIGN  BY  LQR  TECHNIQUE 

The  LQR  method  utilized  with  Qu  =  Q-,-,  =  =  100  and  Q44  =  1  results  in 

a  control  package  with  the  following  equations: 


o,  =  1.0000  v  +  0.0920 1|/  +  0.0393/ 
o,  =  l.OOOOr  +  1.2423  V  +  0.0920/ 


(5.2) 


6br  =  -0.6207v  -  7.1479r  -  3.8547  V  -  4.4499 q 2 satsgn(a]) 

-  12.07 14  q2satsgn(a2) 
6sr  =  1.5414v  +  17.3469r  +  3.2066 i|r;  -  4.6361  q2satsgn(oi) 

+  13.6038  q2satsgn(o2) 


(5.3) 


This  control  package  has  poles  of  -0.3336  and  -0.9480  on  the  sliding  surfaces.  The 
next  step  is  to  design  disturbance  compensation  into  the  controller. 
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D.  STEADY-STATE  DISTURBANCE  COMPENSATION 


Two  methods  of  compensating  for  steady  underwater  currents  will  be  discussed. 
The  first  method  compensates  for  disturbances  by  utilization  of  a  heading  error.  This 
was  the  method  used  in  Chapter  IV  with  LOS  guidance  where  the  vehicle  angles  its 
bow  into  the  current  to  overcome  it.  The  second  method  to  be  discussed  uses  the 
vehicle’s  rudders  to  overcome  the  current.  This  allows  the  vehicle’s  bow  to  remain 
pointed  down  the  track.  This  method  exploits  the  unique  capabilities  of  the  MIMO 
CTE  steering  controller. 

1.  Theory  of  Heading  Error  Compensation 

To  simplify  the  analysis,  the  sliding  surfaces  (5.2)  and  the  control  laws  (5.3) 
are  expressed  in  general  as: 


°1  =  511V  +  V  +  513V  +  SlJ 

°2  =  521V  +  S22r  +  *23  V  + 


(5A) 


5br  =  kuv  +  knr  +  *13  ♦'  +  ki*y  +  fCtsTsatsgnioJ 

+  kl6rf2satsgn(o2 ) 

6sr  =  *21 V  +  k22r  +  *23  +  *24/  +  *25^2 S(HSgH(o ,) 

+  k26r/2satsgn(o2 ) 


(5.5) 


The  reader  should  note  that  ku  =  ^24  =  0,  but  the  equations  will  be  left  in  this  very 
general  form  as  a  function  of  the  entire  state  vector. 
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As  stated  previously,  heading  error  compensation  is  identical  with  the 
procedure  used  in  Chapter  IV  for  the  LOS  controller.  The  reader  should  note  that 
the  following  analysis  is  conducted  at  steady-state  conditions.  The  procedure  starts 
with  steady-state  assumptions  (4.21)  repeated  here  for  convenience: 

*  =  r  -  -  «„  -  0  (5-6> 

Equation  (5.6)  leads  to  a  nonzero  heading  error  at  steady-state,  or  in  other  words  the 
AUV  fights  the  disturbance  by  angling  its  bow  into  the  current,  not  by  using  its 
rudders. 

The  analysis  to  compensate  for  disturbances  is  complicated  by  the 
additional  state  /.  The  requirement  of  realizing  a  CTE  guidance  scheme  results  in 
the  CTE  part  of  (4.16)  becoming: 


y'  =  vc  +  u  sinij/  =  0 

Solving  (5.7),  we  get  the  resultant  steady-state  heading  error: 


-arcsinl  — 
u 


(5.7) 


(5.8) 


Now  we  must  see  if  this  heading  error  has  any  undesired  effects  on  the  controller. 
Equations  (5.6),  (5.7),  and  (5.8)  result  in  the  control  laws  (5.5)  simplifying  to: 
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kiSn2satsgti(al )  +  k16n2satsgn(a2)  =  &13arcsin 
k^sn2satsgn(ax)  +  k2tr)2satsgn(p2)  =  £23  arcsin 


/  \ 

U 

<  \ 

u 


(5.9) 


at  steady-state.  Solving  (5.9)  for  the  two  saturation  functions: 


satsgn(ax) 


1 

/•  £  _  k 

(  \ 
v 

“13*26  *16*23 

arcsin 

c 

0 

n~ 

k  k  —  k  k 

1*15*26  *16*25  J 

/ 

satsgti(o2 ) 


l 

k  k  —  k  k 

*15*23  *13*25 

arcsin 

V 

c 

k  k  —  k  k 
1*15*26  *  16*25  J 

(5.10) 


For  purposes  of  controller  stability,  at  steady-state  we  desire  the  saturation  functions 
to  be: 


satsgn(ox)  £  1 
satsgn( o2)  £  1 

which  establishes  the  lower  limit  on  rf.  Then  it  follows  from  (5.11): 

a. 

satsgn(a)  =  _ 

9 

°2 

satsgn(a,)  =  — 

9 


(5.11) 


(5.12) 
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Combining  (5.10)  and  (5.12),  we  can  solve  for  the  uncompensated  sliding  surfaces  at 
steady-state  in  the  presence  of  a  constant  disturbance: 


a .  = 

0 

k  k 

*13*26 

~  *16*23 

arcsin 

\ 

V 

c 

l 

n~ 

*15*26 

-  *16*25, 

rr  = 

0 

( k  k 

*15*23 

\ 

—  k  k 

13*25 

arcsin 

(  ^ 
V 

c 

2 

-> 

n- 

*15*26 

—  k  k 

*  16*25  J 

(5.13) 


Back  substitution  of  (5.13)  into  the  sliding  surface  equations  (5.4)  yields  two 
expressions  for  the  uncompensated  steady-state  cross-track  error: 


l 

(k  k 

*13*26 

-  k  k  ) 

*16*23 

+  5,  ^ 

arcsin 

\ 

Vc 

n~ 

k  k 

*  15*26 

—  k  k 

*16*25 J 

**13 

(5.14) 


1 

0 

{k  k 

*15*23 

-kk 

*13*25 

+ 

arcsin 

f  \ 

V 

c 

S2A 

n~ 

k  k 

1*15*26 

-  *16*25, 

°23 

Effective  disturbance  compensation  can  be  achieved  by  eliminating  the  effect  of  these 
two  terms.  To  eliminate  (5.13)  and  (5.14)  the  sliding  surfaces  are  augmented  with 
the  additional  terms: 


ai  =  5nv  +  si/*si3^  +si/  + 

ct2  =  s21v  +  s2f  +  s2^'  +  s2y  + 


f  . 

\ 

1 

/  > 

0 

it  k 

*13*26 

—  it  it 

*16*23 

+  513 

arcsin 

V 

c 

r7” 

k  k 

1*15*26 

—  it  it 

*  16*25  J 

/ 

\ 

r  \ 

0 

*15*23 

“  *13*25 

+  523 

arcsin 

V 

c 

f?2 

it  k 

*15*26 

-*16*25, 

(5.15) 
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2.  Simulation  with  Heading  Error  Compensation 


The  observers  for  v,  vc,  and  uc  designed  in  Chapter  IV  can  be  used  for  the 
CTE  controller  with  no  modification.  Programs  SDVCTERR300OBSJLQR.FOR 
(see  Appendix  E)  and  PLOTIO.FOR  (see  Appendix  A)  were  used  for  AUV 
simulation  with  CTE  guidance.  Run  21,  shown  in  Figures  43  and  44,  was  conducted 
with  a  hit  distance  of  0.5L  as  was  used  with  the  LOS  controller.  Global  currents  of 
uco  =  2.0  ft/s  and  vco  =  -2.0  ft/s  operated  on  the  vehicle  starting  at  t  =  0.  Vehicle 
speed  was  u  =  6.0  ft/s  and  controller  parameters  were  <p  =  2.0  and  q2  =  0.35. 

The  AUV  easily  follows  the  straight  path  between  successive  way-points 
even  in  the  presence  of  a  significant  current,  but  the  small  hit  criterion  of  0.5L  causes 
the  AUV  to  overshoot  the  next  leg  in  cases  where  the  next  leg  must  be  reached 
through  a  sharp  angle.  The  worst  case  can  be  seen  in  the  global  plot  in  Figure  43 
at  way-point  (400,400).  The  AUV  overshoots  the  way-point  and  must  reverse  course 
to  fight  the  current  back  to  the  next  leg.  This  wastes  time  and  energy. 

A  simple  but  very  effective  adjustment  is  to  change  the  hit  criterion  to  a 
larger  value.  Run  22  is  a  duplicate  of  Run  21  but  with  a  hit  criterion  of  seven  ship 
lengths  or  7L.  Figures  45  and  46  show  the  simulation  results.  The  new  hit  criterion 
yields  a  vast  improvement  in  the  AUV’s  ability  to  track  the  path.  The  AUV  is  also 
able  to  travel  farther  because  its  path  keeping  has  been  optimized. 

Increasing  the  hit  distance  does  have  a  drawback.  When  the  AUV  must 
turn  through  a  shallow  angle  for  the  next  leg,  7L  is  too  large  a  hit  distance  and  the 
vehicle  heads  for  the  next  leg  too  early.  Hence,  increasing  the  hit  distance  is  a 
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Figure  43.  Run  21  -  AUV  Steering  Response  with  CTE  Guidance,  (Disturbance 

Compensation  by  Heading  Error,  Hit  =  0.5L) 
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Figure  46.  Run  22  -  (continued) 
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compromise.  There  are  alternate  methods  for  modifying  the  AUV’s  control  package, 
but  these  are  beyond  the  scope  of  this  thesis  and  are  the  subject  of  further  study  at 
NPS. 


3.  Theory  of  Rudder  Action  Compensation 

As  alluded  in  Chapter  IV,  an  alternate  method  for  steady-state  disturbance 
compensation  requires  the  AUV’s  rudders  to  overcome  the  underwater  current,  as 
opposed  to  using  a  heading  error  in  the  previous  method.  This  capability  is  an 
inherent  advantage  of  a  MIMO  control  system.  A  SISO  steering  controller  could  not 
achieve  such  action. 

The  design  starts  with  new  steady-state  assumptions: 

r  =  ijr7  =  y1  =  0  (5.16) 

The  CTE  equation  for  /  simplifies  to: 

v  =  -vc  (5.17) 


and  using  (5.17)  in  (5.1)  yields: 


*1. 

<5br 
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12  sr 

=  °llVc 

*21 
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b„6 

22  sr 

II 

tJ 

■c 

o 

(5.18) 


Treating  (5.18)  as  two  equations  with  two  unknowns,  we  solve  for  the  steady-state 
rudder  deflections  needed  to  overcome  the  underwater  current: 
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(5.19) 


br 


«.r  - 


^  1 1  ^22 

-  a2\bn 

b  nb  22 

^21^12, 

a2ibn 

anb2i 

bl\b22 

~  b2\ b \2j 

As  before,  we  must  analyze  the  resultant  effect  on  the  sliding  surfaces  and  control 
laws  at  steady  state.  Substitution  of  (5.19)  into  the  control  laws  (5.5)  allows  the 
determination  of  the  uncompensated  sliding  surfaces  at  steady-state  in  the  presence 
of  an  underwater  current: 


V  0 
c  ~ 

(^11^26  ^16^21^  ^  ^2(,(a i{b22  a2lb\z)  ^I6^02\b ll  °11^2l) 

0 

n~ 

(^15^26  ~  ^16^25^  (^15^26  ~  ^16^25^  (^11^22  ~  ^12^2l) 

(5.20) 


vc<t> 

(^11^25  ^"15^21^  ^  ^2s(fl11^22  ~  ^21^12)  ^15(^21^11  fl11^2l) 

rr 

(^16^25  ~  ^15^26^  (^16^25  ~  ^15^26^  (^11^22  ~  ^12^2l) 

Writing  (5.20)  as: 

a\  ~  °i 

a,  =  c,‘ 

Substituting  (5.21)  into  the  sliding  surface  equations  (5.4)  results  in  two  equations  for 
the  uncompensated  CTE  at  steady-state: 


(5.21) 
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ai  +  *nvc 
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°2  *  S2 iVc 


’24 


(5.22) 


Finally,  we  compensate  the  sliding  surface  equations  to  counteract  (5.21)  and  (5.22): 


ti  =  5nv  +  snr  +  s  a** +  +  °i + 

r  2  =  S2lV  +  522r  +  S23^  +  524  /  +  °2  +  S2l] 


(5.23) 


4.  Simulation  with  Rudder  Action  Compensation 

To  analyze  the  effectiveness  of  rudder  disturbance  compensation,  Run  23 
was  conducted  at  u  =  6.0  ft/s,  <p  =  2.0,  and  p2  =  0.35  with  a  local  lateral  current  of 
vc  =  -1.0  ft/s.  Figures  47  and  48  show  Run  23  which  was  produced  by  simulation 
program  SDVCTELOSRR300OBS  LQR.FOR  (see  Appendix  A)  and  DISSPLA 
plotting  program  PLOTIO.FOR.  To  easily  demonstrate  the  effectiveness  of  the 
compensation,  the  simulation  starts  at  i|r  =  45°  at  way-point  (0,0)  headed  towards  the 
next  way-point  at  (1000,1000).  At  t  =  0,  the  lateral  current  is  initiated  on  the  AUV. 
The  vehicle  parameters  oscillate  slightly  as  the  observers  and  controller  compensate 
for  the  step  in  current.  It’s  the  steady-state  values  that  must  be  closely  scrutinized. 
The  AUV  settles  out  at  approximately  i|r  =  45°  as  desired!  The  rudders  are  both 
deflected  in  the  same  direction  to  combat  the  current.  The  bow  rudder  is  saturated 
at  6br  =  0.4  and  the  stern  rudder  is  at  6sr  =  0.3.  If  the  bow  rudder  did  not  saturate, 
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Figure  47.  Run  23  -  AUV  Steering  Response  with  CTE  Guidance,  (Disturbance 

Compensation  by  Rudder  Action,  vc  =  -1.0  ft/s) 
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the  AUV  would  achieve  a  heading  angle  of  exactly  45°.  The  difference  in  the  rudder 
deflections  is  both  due  to  the  forward-aft  asymmetry  of  the  AUV’s  (actually  the 
SDV’s)  body  and  the  coefficients  in  the  control  laws.  Note  that  the  sway  velocity  v 
settles  out  at  about  1.0  ft/s  as  predicted  by  (5.17). 

For  comparison,  Run  24  on  Figures  49  and  50  have  been  included.  Run 
24  was  conducted  with  the  same  conditions  as  Run  23,  but  the  controller  in  Run  24 
uses  heading  error  disturbance  compensation  discussed  previously.  Note  that  the 
rudders  have  zero  deflection  at  steady-state,  but  the  AUV  settles  out  at  about 
i|f  =  55°  which  is  a  10°  heading  error.  Hence  in  comparing  Runs  23  and  24,  rudder 
disturbance  compensation  eliminates  a  10°  heading  error. 

Taking  a  further  look  at  Run  23  we  note  again  that  with  a  lateral  current 
of  only  -1.0  ft/s,  one  rudder  is  saturated  and  the  other  is  close  to  saturation. 
Obviously,  we  expect  the  controller  will  be  unable  to  compensate  for  a  larger  lateral 
current.  Run  25  was  conducted  to  investigate  the  response  of  the  CTE  controller 
with  the  rudder  method  of  disturbance  rejection  in  the  presence  of  a  larger  current 
of  vc  =  -2.0  ft/s.  It’s  interesting  to  note  on  Figures  51  and  52  of  Run  25  that  the 
controller  does  the  best  job  it  can  of  reducing  the  heading  error.  The  rudders  are 
deflected  over,  but  to  achieve  zero  cross-track-error  the  AUV  must  also  have  a 
heading  error  of  approximately  10°  (\Jr  =  55°). 

Run  26,  shown  on  Figures  53  and  54,  was  included  for  comparison  with 
Run  25.  The  controller  used  in  Run  26  uses  heading  error  disturbance  compensation. 
Note  that  the  AUV  settles  out  at  a  heading  of  about  ifr  =  65°  for  a  heading  error  of 


121 


<5br  &  <5Sr  (RADIANS)  X-AXIS  (FT) 

0.4  -0.2  0.0  0.2  0.4  0  200  400  600  800  1000 


Figure  49.  Run  24  -  AUV  Steering  Response  with  CTE  Guidance,  (Disturbance 

Compensation  by  Heading  Error,  vc  =  -1.0  ft/s) 
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Figure  51.  Run  25  -  AUV  Steering  Response  with  GTE  Guidance,  (Disturbance 

Compensation  by  Rudder  Action,  vc  =  -2.0  ft/s) 
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Figure  53.  Run  26  -  AUV  Steering  Response  with  CTE  Guidance,  (Disturbance 

Compensation  by  Heading  Error,  vc  =  -2.0  ft/s) 
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Figure  54.  Run  26  -  (continued) 
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about  20°.  Thus,  the  CTE  controller  with  rudder  disturbance  compensation,  although 
unable  to  fully  eliminate  the  heading  error,  does  reduce  it  significantly. 

E.  CONCLUDING  REMARKS 

A  cross-track-error  guidance  steering  controller  has  been  designed  for  the  AUV 
which  also  uses  the  observers  designed  in  Chapter  IV.  The  CTE  controller  effectively 
follows  a  path  in  the  horizontal  plane.  One  difficulty  is  the  criterion  for  a  hit  of  a 
way-point.  If  the  distance  for  a  hit  is  too  small  when  the  next  leg  must  be  reached 
through  a  large  angle,  significant  overshoot  occurs  after  passing  through  the  way- 
point.  If  too  large  a  distance  is  used  when  the  following  leg  must  be  reached  through 
a  shallow  angle,  the  AUV  heads  for  the  next  leg  too  early.  Thus,  the  hit  distance  is 
a  compromise  reached  through  analysis  of  the  AUV’s  ordered  speed  and  prospective 
track. 

It  should  be  emphasized  that  in  the  MIMO  design  adopted  here,  the  sway 
velocity  v  assumes  significant  values  and,  therefore,  becomes  important  in  the  control 
law  design.  This  is  in  contrast  to  SISO  designs  where  v  is  negligibly  small  and  can  be 
neglected. 

Disturbance  compensation  was  designed  into  the  controller  by  adding  a  term 
to  each  sliding  surface.  Two  methods  of  disturbance  compensation  were  utilized. 
First,  the  AUV  overcame  the  current  with  a  heading  error.  The  advantage  of 
heading  error  compensation  is  its  effectiveness  over  a  wide  range  of  underwater 
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currents.  The  second  method  of  rudder  compensation  will  keep  the  AUV  pointed 
along  the  track,  but  it’s  unable  to  compensate  for  lateral  currents  above  1.0  ft/s. 

The  final  step  of  this  thesis  is  to  apply  the  steering  and  diving  controllers,  which 
were  designed  separately,  to  the  full  nonlinear  model. 
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VI.  AUV  SIMULATION  IN  THREE  DIMENSIONAL  SPACE 


A.  INTRODUCTION 

In  previous  chapters,  simulation  of  the  AUV  was  conducted  with  only  simplified 
nonlinear  equations  of  motion.  To  prove  the  validity  of  the  diving  and  steering 
controllers  which  were  designed  based  on  these  simplified  nonlinear  equations  of 
motion,  control  of  the  AUV  must  be  simulated  using  the  full  nonlinear  equations 
developed  by  Smith,  Crane,  and  Summey  [Ref.  7].  First,  the  AUV  will  be  controlled 
with  the  dMng  and  LOS  controllers.  Second  the  CTE  controller  will  be  used  vice  the 
LOS  controller.  Finally,  three  dimensional  path  keeping  will  be  developed  and 
simulated. 

B.  SIMULTANEOUS  LOS  STEERING  AND  DEPTH  CONTROL 

Figures  55  and  56  show  Run  27  which  was  produced  by  program 
SDV3D  LOS.FOR  and  DISSPLA  program  PLOT3DLOS.FOR  (see  Appendix  A). 
The  diving  controller  was  executed  with  <£  =  2.0  and  q2  =  0.2,  the  LOS  controller 
with  <p  =  2.0  and  q1  =  0.3,  and  the  speed  controller  with  <p  =  1.0.  Ordered  speed 
was  uD  -  6.0  ft/s  and  global  currents  were  uco  =  -2.0  ft/s  and  vco  =  2.0  ft/s.  The  hit 
criterion  was  one  vehicle  length.  Perfect  state  feedback  was  used. 

Figures  55  and  56  show  that  the  controllers  do  an  excellent  job  of  vehicle 
control  even  during  simultaneous  depth  and  course  changes.  Movement  of  the 
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Figure  55.  Run  27  -  AUV  Simulation  During  Simultaneous  Course  and  Depth 

Changes,  LOS  Steering  Controller,  Perfect  State  Feedback 


131 


60  100  160  200  260 

T  (SEC) 


SOLip46BP 
DASfjH  <5«p 


60  100  160  200  250 

T  (SEC) 


• 

i 

O  ' 
evj  - 

o  ■ 

i 

60  100  160  200  260 

60  100  150  200  250 

T  (SEC) 

T  (SEC) 

132 


control  surfaces  is  smooth  with  no  oscillation.  Maximum  vehicle  roll  is  less  than  2°. 
Of  course,  the  AUV  does  overshoot  way-point  (500,500)  due  to  the  combined  effect 
of  the  small  hit  criterion  and  the  vco  component  of  the  current.  The  reader  should 
note  that  the  simulation  ends  before  the  vehicle  reaches  the  final  ordered  way-point. 

The  conclusion  is  that  a  combined  control  package  containing  the  diving  and 
LOS  steering  controllers  is  effective  for  AUV  control  during  simultaneous  course  and 
depth  changes. 

C.  SIMULTANEOUS  CTE  STEERING  AND  DEPTH  CONTROL 

Figures  57  and  58  show  Run  28  which  was  produced  by  program 
SDV3D  CTE.FOR  and  DISSPLA  program  PLOT3D  CTE.FOR  (see  Appendix  A). 
The  only  change  from  the  previous  run  was  the  CTE  steering  controller  was  executed 
with  q2  -  0.35  and  the  hit  criterion  was  increased  to  six  vehicle  lengths. 

Figures  57  and  58  reveal  that  the  CTE  steering  and  diving  controllers  are 
effective  in  controlling  the  AUV.  The  movement  of  the  rudders  is  not  as  smooth  as 
the  previous  run  with  the  LOS  controller.  This  is  primarily  due  to  the  CTE  guidance 
scheme  which  requires  strong  control  action  to  achieve  minimal  /.  Note  that  the 
increased  control  surface  action  results  in  a  maximum  roll  angle  of  about  5°  which  is 
3°  larger  than  Run  27. 

D.  THREE  DIMENSIONAL  VEHICLE  PATH  KEEPING 

So  far,  path  keeping  in  the  horizontal  plane  has  been  coupled  with  depth 
keeping  in  the  vertical  plane.  More  complex  obstacle  avoidance  and  path  planning 
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Figure  57.  Run  28  -  AUV  Simulation  During  Simultaneous  Course  and  Depth 

Changes,  CTE  Steering  Controller,  Perfect  State  Feedback 
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situations  may,  however,  require  that  the  vehicle  can  accurately  follow  a  path  defined 
in  a  three  dimensional  coordinate  frame.  To  this  end,  we  must  first  consider  the 
problem  of  path  keeping  in  the  vertical  plane. 

1.  Path  Keeping  in  the  Vertical  Plane 

The  linearized  equations  of  motion  in  the  vertical  plane  of  a  neutrally 
buoyant  symmetric  vehicle  with  xG  =  xB  =  0,  zB  =  0,  and  zG  >  0  are: 


(m-ZJvv  -  Z  q 


=  ( Zq+m)uq  + 


Z  uw  +  Z,  u26 


Jsp 


sp 


Zr  U26 
6  bp  bP 


(6.1) 


(/y  ~Mq)q  -  M*w  =  Mqiiq  +  Mjiw  -  zGmg sin0  +  M&sU26sp 


+  Mr  u26. 

5 bp  bP 


(6.2) 


0  -q 

x  =  u  cosQ  +  w  sin0 


z  -  -u  sin0  +  w  cos0 


(6.3) 

(6.4) 

(6.5) 


where  the  pitch  angle  d  is  not  necessarily  small,  and  the  coordinate  system  is  shown 
in  Figure  59.  The  assumption  that  zG  >  0  ensures  that  the  vehicle  is  stable  in  roll, 
which  is  normally  the  case.  If  fi  denotes  the  commanded  pitch  angle,  positive  nose 
up,  we  rotate  the  coordinate  system  as  shown  in  Figure  59  and  we  get: 

z'  =  z  cos  (3  +  x  sin  3 
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x 1  =  -zsinR  +  xcosft 

(6.7) 

0  =  3  +  07 

(6.8) 

where  O'  is  a  small  angle,  defined  as  the  deviation  between  the  actual  and  the 
commanded  pitch  angle,  and  z'  is  the  cross-track-error  off  the  assumed  straight  line 
path.  Then  the  linearized  equations  of  motion  in  the  vertical  plane  in  the  (jc7,  z ') 
system  of  coordinates  become: 


e'  =  q 

(6.9) 

w  =  anuw  +  anuq  +  b^urS 

(6.10) 

q  =  a1{uw  +  a„uq  +  o2307  +  b,u26  +  d 

(6.11) 

z‘  =  -u  07  +  w 

(6.12) 

where  we  have  set  6sp  =  6,  6bp  =  0  for  now,  and  the  coefficients  are  given  by: 


(m  ~  Zw) (/y  -  Mq )  -  ZqA/* 

(7y-A/q)(Zq+m)  +  ZqMq 
(m-ZJ(7y-Mq)  -  ZqA7* 


(6.13) 


(6.14) 
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(6.15) 
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(6.16) 

(6.17) 

(6.18) 

(6.19) 

(6.20) 


The  sliding  mode  control  law  for  system  (6.9)  through  (6.12)  is  expressed  as: 

6  =  kfl  +  k,w  +  k^q  +  knsatsgn(o )  (6.21) 

CT  =  Jj©7  +  s,*v  +  53^  +  s4z'  +  $5  (6.22) 


The  gains  kx,  k2,  k2  and  the  sliding  plane  coefficients  s„  s2,  s3,  s4  can  be  computed 
using  either  the  LQR  or  pole  placement  techniques  as  developed  in  Chapter  II.  It 
should  be  mentioned  that  since  the  coefficient  a2 3  in  (6.17)  depends  on  the 
commanded  pitch  angle  /3,  the  above  gains  depend  on  (i  as  well.  Therefore,  they 
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have  to  be  recomputed  every  time  a  new  angle  ft  is  commanded.  It  was  found, 
however,  that  their  variation  in  ft  is  small  and  a  constant  average  angle  ft  can  be  used 
in  the  control  law  design. 

The  extra  coefficient  s5  in  (6.22)  appears  because  of  the  nonzero 
metacentric  height,  or  the  constant  term  d  in  (6.11).  This  feedforward  term  ss  can 
be  computed  from  steady-state  accuracy  requirements.  At  steady-state,  equations 
(6.9)  through  (6.12)  yield: 


6  = 


and 


a2lblU 2  +  Q23bl  ~  anb2ul 


(6.23) 


O'  = 


~bxd 


02\b\U 2  +  a23bl  ~  a\\b->U 2 


(6.24) 


w 


-bxud 


a2ib\u 2  +  (*i3bl  -  aub2u2 


(6.25) 


Equation  (6.23)  provides  the  dive  plane  angle  that  is  necessary  to  achieve  the 
commanded  rise  or  dive  angle,  and  equations  (6.24)  and  (6.25)  reveal  that  this  is  only 
possible  with  a  nonzero  heave  velocity  and  path  orientation  error.  Then,  equation 
(6.21)  yields: 


satsgn(a ) 


(an  +  kxbx  +  k2bxu)d 
kn(a2ibiu2  +  a23bi  ~  anb2u 2) 


(6.26) 


and  using: 
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together  with  the  requirement  that  the  cross-track-error  z'  reaches  zero  at  steady- 
state,  we  get  from  (6.22): 


(au  +  kjh,  +  k,blu)<pd  +  kn(sl  +  s^u)bxd 
kniP2xbxu2  +  a23bl  -  anb2u2) 


(6.28) 


Equation  (6.27)  is  valid  if  |  satsgn(a)  |  <  1,  and  this  yields  the  critical  value  of  kn  for 
stability: 

(ou  +  klbl  +  k^bxii)d 
+  a,3bl  -  anb,u2 


The  above  scheme  will  provide  the  desired  response  once  the  vehicle 
hydrodynamics  and  geometric  properties  are  accurately  known.  In  cases  of  actual 
vehicle/mathematical  model  mismatch,  a  steady-state  path  error  will  develop.  This 
is  especially  true  for  different  vehicle  loading  conditions  which  change  the  value  of 
the  metacentric  height  from  its  nominal  design  value.  An  integrator  in  z'  (with  a 
sufficiently  large  time  constant  to  avoid  undesirable  oscillations)  or  an  estimator  of 
zG  can  be  incorporated  into  the  design  to  ensure  the  required  steady-state  accuracy. 
For  typical  cases  of  the  SDV  applications  considered  in  this  work,  this  did  not  seem 
to  be  necessary  since  the  actual  steady-state  path  errors  appear  to  be  very  small. 
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This  is  demonstrated  in  Figure  60.  The  vehicle  is  initially  horizontal  and  the 
commanded  way-point  is  located  at  (x,y)  =  (15,  -15)  ship  lengths,  which  corresponds 
to  a  45°  rise  angle.  Curve  1  simulates  the  nominal  design  with  zG  =  0.1  ft,  while 
Curve  2  simulates  the  case  where  the  actual  zG  is  three  times  larger  than  the  assumed 
value,  and  in  the  case  of  Curve  3  it  is  five  times  smaller.  It  can  be  seen  that  even 
under  such  a  wide  variation,  the  actual  path  error  is  negligible. 

When  both  bow  and  stern  planes  are  acting,  the  linearized  equations  (6.9) 
through  (6.12)  become: 

e'  =  q  (6-3°) 

w  =  aniiw  +  anuq  +  buu26sp  +  bnu26bp  (6-31) 

q  =  a1{uw  +•  a2^uq  +  6,1«26sp  +  6,,w26bp  +  n^O7  +  d  (6.32) 

z'  =  -u&  +  w  (6.33) 

and  the  control  laws: 

6sp  =  kn&  +  kuw  +  knq  +  klAqzsatsgn{ax)  +  k  15p2  satsgn(a  2)  (6-34) 

6bp  =  *21  ^  +  *22 W  + 


k23q  +  k24n2satsgn(ox )  +  k25n2satsgn(a2)  (6-35) 


'  0.  0  5.  0  10.0 

X 


Figure  60.  Path  Keeping  in  the  Vertical  Plane  with  Stern  Planes  Only 
(zG  =  0.1  ft  (nominal),  zG  =  0.3  ft,  and  zG  =  0.5  ft) 


15.0 
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with: 


°x  =  ^i07  +  Ji2w  +  s\tf  +  suz'  +  %  (6‘36) 

o,  =  +  s22w  +  s22q  +  s24z'  +  s25  (6.37) 


The  existence  of  two  independent  inputs  can  now  ensure  zero  steady-state  path 
deviation  with  zero  heave  velocity  and  the  commanded  pitch  angle  as  well.  Equations 
(6.30)  through  (6.33)  yield  at  steady-state: 

4  =  8'  =  *  =  0 

6  =  bn<i 

SP  (bnb22  ~  bnb2\)u2 

6  b"d 
bP  ~  (*11*22  ~  *12*21)“'’ 


(6.38) 

(6.39) 

(6.40) 


Then,  using  equations  (6.34)  through  (6.37)  we  get: 

(*12^25  +  *ll^is)^^ 
n~U~(bnb22  “  *12*21  )(^14^25  ~  ^15^24) 

“(*11^14  +  *12^24  ) 

r)~u~(bub22  -  bl2b2l)(kuk25  -  kl$k24) 


(6.41) 


(6.42) 
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and  the  control  law  is  complete.  Results  for  the  SDV  at  u  =  2.5  ft/s,  using  stern 
planes  only  (Curves  1)  and  combined  stern  and  bow  planes  (Curves  2)  are  presented 
in  Figures  61  and  62.  It  can  be  clearly  seen  that  the  application  of  the  multiple  input 
sliding  mode  technique  leads  to  a  much  better  steady-state  accuracy. 

2.  Three  Dimensional  Path  Keeping 

Cross-track-error  controls  in  the  horizontal  and  vertical  planes  can  be 
coupled  together  to  provide  accurate  vehicle  path  keeping  in  three  dimensions.  The 
related  geometry  is  illustrated  in  Figure  63  along  with  some  nomenclature.  The 
angles  aH  and  av  are  the  two  coordinate  rotation  angles  to  align  a  local  frame  to  the 
desired  vehicle  path.  At  the  same  time,  «v  is  the  vehicle  commanded  pitch  angle. 
The  horizontal  plane  cross-track-error  /  is  controlled  by  the  rudders  while  the 
vertical  plane  cross-track-error  z7  is  driving  the  dive  planes.  The  two  coordinate 
rotations  are  shown  in  Figure  64  where  the  horizontal  plane  rotation  is  executed  first, 
and  then  the  vertical  plane  rotation  follows.  The  geometric  relations  that  describe 
the  above  rotations  are: 


aH  =  arctanl 


-ya 


iXD  *o. 


*  =  (y  -  >'0)sinaH  +  ( X  ~  *o)COSaH 


/  =  (y  -  yo)C0SaH  -  (*  -  *o)sinaH 


(6.43) 


(6.44) 

(6.45) 
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Figure  61.  Runs  29  and  30  *  Comparison  of  Path  Keeping  in  the  Vertical  Plane 
with  Stem  Planes  Only  (Curves  1)  and  Stem  &  Bow  Planes  (Curves  2) 
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(6.46) 


av  =  arctan 


x'd  =  (yD  -  y0)sinaH  +  ( *D  -  *0)cosaH 

(6.47) 

x//  =  -(z  -  zo)sinav  +  j/cosav 

(6.48) 

z'  =  (z  -  zo)cosav  +  x/sinav 

(6.49) 

Results  are  presented  for  the  following  way-points  (x,  y,  z)  =  (10,  0,  5), 
(20,  5,  5),  (30,  -5,  -3),  (50,  0,  -5)  vehicle  lengths,  using  one  stern  rudder  and 
independent  stern  and  bow  planes  as  developed  before,  in  Figures  65  through  67. 
The  commanded  vehicle  forward  speeds  are  3.0,  2.0,  and  1.5  ft/s,  respectively,  and  the 
target  distance  was  fixed  at  2  ship  lengths.  The  commanded  and  actual  vehicle  paths 
are  plotted  projected  on  the  three  planes  (x,  y),  (x,  z),  and  (y,  z).  As  can  be  seen  the 
above  coordinate  rotations  provide  excellent  path  keeping  capabilities  except  for  the 
low  speed  of  1.5  ft/s  where  the  dive  planes  cannot  always  provide  the  necessary  dive 
and  rise  moment.  At  such  low  speeds,  however,  the  vertical  thrusters  are  to  be 
turned  on,  thus  providing  additional  heave  force  and/or  pitch  moment. 

E.  CONCLUDING  REMARKS 

The  diving  and  steering  controllers  designed  in  previous  chapters  were 
combined  to  perform  simultaneous  depth  and  steering  control.  Both  the  LOS  and 
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CTE  steering  controllers  performed  satisfactorily  in  conjunction  with  the  diving 
controller. 

The  new  concept  of  three  dimensional  path  keeping  was  introduced. 
Development  of  the  concept  yields  a  control  package  that  allows  the  AUV  to  follow 
a  straight  path  line  in  three  dimensional  space. 
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VII.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

The  following  are  the  major  conclusions  that  can  be  drawn  from  this  study: 

•  MIMO  sliding  mode  controllers  yield  superior  performance  and  enhanced 
capabilities  as  compared  to  SISO  sliding  mode  controllers. 

•  The  LOR  method  for  MIMO  sliding  mode  control  design  is  preferred  over  pole 
placement  because  it  allows  the  designer  to  choose  which  states  to  minimize 
resulting  in  specific  desired  control  action.  However,  even  the  LQR  method 
requires  some  experimentation  to  determine  the  best  minimization  matrix  for 
the  situation.  Hence,  this  is  an  impetus  for  prior  computer  simulation. 

•  As  first  proposed  by  Lienard  [Ref.  3],  the  speed,  diving,  and  steering  controllers 
can  be  designed  separately  -  greatly  simplifying  the  design  process  -  and 
effectively  control  the  vehicle  simultaneously. 

•  The  multi-level  diving  controller  based  on  control  efficiency  at  different  speeds 
proved  that  it  can  effectively  command  the  vehicle  over  the  entire  range  of 
operational  speeds. 

•  Both  the  LOS  and  CTE  steering  controllers  did  well  in  controlling  the  vehicle. 
They  can  be  easily  modified  to  compensate  for  observed  underwater  currents. 

•  Two  coupled  CTE  controllers  can  command  the  vehicle  to  follow  straight  line 
paths  in  three  dimensional  space;  however,  consideration  must  be  given  to  the 
mission  planner  so  the  ordered  path  is  achievable.  The  vehicle  is  unable  to 
follow  a  path  line  that  is  too  steep. 


B.  RECOMMENDATIONS 

The  following  actions  should  be  taken  to  fully  utilize  the  research  done  in  this 
thesis: 
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•  Design  Kalman  filters  to  reduce  noise  in  the  state  measurements. 

•  Develop  a  more  accurate  and  detailed  model  for  the  tunnel  thrusters. 

•  Add  horizontal  tunnel  thrusters  to  the  model  and  design  a  multi-level  steering 
controller,  as  was  done  for  the  diving  controller. 

•  Develop  software  for  switching  of  dive  planes  and  rudders  to  ensure  smooth 
transition  between  different  way-points  during  three  dimensional  path  keeping. 
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APPENDIX  A.  SUMMARY  LISTING  OF  COMPUTER  PROGRAMS 


The  following  is  a  complete  listing  of  all  computer  programs  written  and  used 
in  this  thesis  in  the  order  they  were  discussed.  The  four  programs  contained  in 
Appendices  B  to  E  are  included  because  they  are  in  the  most  general  form  for  the 
particular  application.  None  of  the  DISSPLA  plotting  programs  are  included  in  the 
Appendices  because  they  are  device  dependent,  but  they  are  summarized  below.  All 
Fortran  simulation  programs  utilize  IMSL  subroutines  for  matrix  inversion  and 
matrix/vector  multiplication.  Copies  of  any  program  used  in  this  thesis  can  be 
obtained  from: 


Professor  F.  A.  Papoulias,  Code  MEPa 
Department  of  Mechanical  Engineering 
Naval  Postgraduate  School 
Monterey,  CA  93943-5000 


The  listing  follows: 


SDVDIVLINJ.FOR 

Fortran  program  that  solves  the  simplified  nonlinear  heave  and  pitch  equations 
(with  planes  and  vertical  thrusters)  for  w  and  q  at  a  particular  surge  velocity 
u. 


SDVDIVPPTTPSFJPOLE.FOR 

Fortran  program  that  simulates  the  AUV  at  all  speeds  (with  planes  and 
thrusters)  in  the  dive  plane  only,  via  the  simplified  nonlinear  equations  of 
motion.  Control  laws  determined  by  pole  placement.  Perfect  state  feedback. 
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SDVDrVPPTTPSF_LQR.FOR 

Identical  to  SDVDIVPPTTPSFPOLE.FOR  except  the  control  laws  are 
determined  by  the  LQR  method. 

DPLOT2.FOR 

Fortran/D ISSPLA  plotting  program  that  produces  graphs  from  the  output  of 
SDVDIVPPTTPSF  POLE.FOR  and  SDVDIVPPTTPSF_LQR.FOR. 

SDVDIVPPTTOBS_LQR.FOR 

Identical  to  SDVDIVPPTTPSF_LQR.FOR  except  it  has  a  reduced  order 
observer  for  heave  velocity  w. 

DPLOT4.FOR 

Fortran/DISSPLA  plotting  program  that  produces  graphs  from  the  output  of 
SDVDIVPPTTOBSLQR.FOR. 

SDVSTRLIN.FOR 

Fortran  program  that  solves  the  simplified  nonlinear  sway  and  yaw  equations 
(with  two  rudders)  for  v  and  f  at  a  particular  surge  velocity  u. 

SDVLOSRR300PSF_LQR.FOR 

Fortran  program  that  simulates  the  AUV  at  high  speeds  (with  two  rudders)  in 
the  horizontal  plane  only,  via  the  simplified  nonlinear  equations  of  motion. 
Control  laws  determined  by  the  LQR  method.  State  space  representation 
determined  from  linearization  at  u  =  3.00  ft/s.  Perfect  state  feedback. 
Compensated  for  steady-state  disturbances. 

PLOT7A.FOR 

Fortran/DISSPLA  plotting  program  that  produces  graphs  from  the  output  of 
SDVLOSRR300PSFLQR.FOR. 

SDVLOSRR300OBS_  LQR.FOR 

Identical  to  SDVLOSRR300PSF  LQR.FOR  except  it  has  a  reduced  order 
observer  for  v,  uc,  and  vc. 
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PLOT8.FOR 


Fortran/DISSPLA  plotting  program  that  produces  graphs  from  the  output  of 
SDVLOSRR300OBSLQR.FOR. 

SDVCTERR300OBS_LQR.FOR 

Fortran  program  that  simulates  the  AUV  with  CTE  steering  guidance  at  high 
speeds  (with  two  rudders)  in  the  horizontal  plane  only,  via  the  simplified 
nonlinear  equations  of  motion.  Control  laws  determined  by  the  LQR  method. 
State  space  representation  determined  from  linearization  at  u  =  3.00  ft/s. 
Reduced  order  observer  for  v,  uc ,  and  vc.  Compensated  for  steady-state 
disturbances  by  heading  error. 

SDVCTELOSRR300OBS_LQR.FOR 

Identical  to  program  SDVCTERR300OBSLQR.FOR  except  that  steady-state 
disturbances  are  compensated  for  by  rudder  action. 

PLOTIO.FOR 

Fortran/DISSPLA  program  that  produces  graphs  from  the  output  of 
SDVCTERR300OBSLQR.FOR  and  SDVCTELOSRR300OBS_LQR.FOR. 

SDV3D_LOS.FOR 

Fortran  program  that  simulates  the  AUV  with  the  full  nonlinear  equations  of 
motion  at  high  speeds  only  (two  rudders/two  planes).  Combined  controller 
containing  the  diving  and  LOS  steering  control  laws.  Perfect  state  feedback. 
Compensated  for  steady-state  disturbances. 

PLOT3D_LOS.FOR 

Fortran/DISSPLA  program  that  produces  graphs  from  the  output  of 
SDV3DLOS.FOR. 

SDV3D_CTE.FOR 

Identical  to  program  SDV3D_LOS.FOR  except  the  combined  controller 
contains  the  CTE  steering  control  laws. 
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PLOT3D_CTE.FOR 

Fortran/DISSPLA  program  that  produces  graphs  from  the  output  of 
SDV3D_CTE.FOR. 
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APPENDIX  B.  MATRIX-x  LQR  DESIGN  PROGRAM  FOR  MIMO  SLIDING 
MODE  CONTROLLERS 


1 


A=[-0. 02270806,  -0.05053369,  2.5100E-2,  0.0; 

0.00562727,  -0.06273627,  -6.6760E-2,  0.0; 

0.0,  1.0,  0.0,  0.0; 

1.0,  0.0,  -0.375,  0.0]; 

B=[  0.00017068,  0.00013609; 

.00004859,  0.00004343; 

J.0,  0.0; 

0.0,  0.0|; 

A1  l=A([  1  2],[1  2)); 

A12=A(|1  2], [3  4]); 

A21=A((3  4|,[  1  2]); 

A22=A([3  4], (3  4]); 

B1=B([1  2j,[  1  2]); 

INQUIRE  Qll 
INQUIRE  Q22 
INQUII  E  Q33 
INQUIRE  Q44 

Q=DIAGQNAL(|Q1 1  Q22  Q33  Q441); 

Q1 1=Q([1  2),[1  2]); 

Q12-Q([1  2), [3  41); 

Q21=Q([3  41,(1  2)); 

Q22=Q((3  41,(3  4]); 

QSTAR=Q22-Q21  *INV(Q1 1)*Q12; 

ASTAR=A22- A21*INV(Q11)*Q12; 

[EVAL,  KR,P|=REGULATOR(ASTAR,  A21,QSTAR,Q1 1); 
C2PRIME=INV(Q1 1)*(Q12+A21’*P); 

C1PRIME=EYE(2); 

CPRIME=[C1PRIME,C2PRIME], 

Y1GAIN=-INV(B1)*(A  1 1+C2PRIME*  A21 ;; 

Y2GAIN  =  -TNV(B1)*(A12+C2PRIME*A22); 

YGAIN={  V1GAIN,Y2GAIN|; 

XGAIN=YGAIN 
SPRIME=CPRIME 
PQLES=EV  AL 
NEGINVB1  =  -INV(B1) 
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APPENDIX  C.  AUV  DIVING  SIMULATION  PROGRAM;  MIMO  SLIDING 
MODE  CONTROLLER  DESIGNED  BY  LQR  METHOD;  W 
OBSERVED 


c 
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PROGRAM  SDVDI VPPTTOBS _ LQR 


PROGRAM  SDVDIVPPTTOBS_LOR.FOR 
WRITTEN  BY  L T  TODD  D.  HAWKINSON,  USN 
FOR  AUV  THESIS  WORK 

THIS  PROGRAM  CONTROLS  A  2-PLANE,  2-THRUSTER  SDV  IN  THE 
VERTICAL  PLANE  WITH  SLIDING  MODE  CONTROL  UTILIZING  PLANES 
AND  THRUSTERS. 

SPEED  IS  ALSO  CONTROLLED  USING  SLIDING  MODE  CONTROLLER 
DEVELOPED  BY  LIENARD. 

THIS  PROGRAM  OBSERVES  HEAVE  VELOCITY,  W. 

THE  OUTPUT  OF  THIS  PROGRAM  IS  WRITTEN  TO  FILE,  ALLOUT.DAT. 

THIS  FILE  IS  THEN  ACCESSED  BY  PROGRAM,  DPLOT4.FOR,  TO  GENERATE 
OUTPUT  GRAPHICS. 

THIS  PROGRAM  UTITLIZES  SUBROUTINES  FROM  THE  IMSL  MATH 
LIBRARY,  VERSION  1.1,  (COPYRIGHT  JANUARY  1989  BY  IMSL,  INC  ). 


HEAVE  CHARACTERISTICS 

REA L  ZQ,ZODOT,ZW,ZWDOT,ZDB,ZDS,CDZ,Z.ZDOT,W.WDOT 
PITCH  CHARACTERISTICS 

REAL  MO,MODOT,MW,MWDOT,MDB,MDS,THE,THEDOT,O.QDOT 
VEHICLE  CHARACTERISTICS 

REAL  BOY,WT,L,XB,XG,ZB,ZG,IY,RHO,G.M,X(18),B(18),NU 
SURGE  CHARACTERISTICS 
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REAL  U,UDOT,UMAX,UCF,UD,N,RE,CDO,XUDOT,XWDB, 

&  XWDS,XQDB,XQDS,XWW,XQQ,XWQ,XDBDB,XDSDS,SIGNU 

PLANE  CHARACTERISTICS 

REAL  DMAX,DB,DS 

THRUSTER  CHARACTERISTICS 

REAL  IBV,ISV,I VM AX,XBV,XSV 

SLIDING  MODE  CONTROLLER  VARIABLES 

REAL  SI,EITA,S1  ,S2,S3,SAT1,SAT2,SAT3 

OBSERVER  VARIABLES 

REAL  Zl.WHAT 
COMMON  Z1 

PROGRAM  VARIABLES 

REAL  MM(3,3),MMINV(3,3),F(3) 

REAL  DELT.PIE 
REAL  SGN 

REAL  VEC1(18),VEC2(18) 

REAL  DRAGDX,DRAGXDX 
REAL  TIME, ZD 

INTEGER  I, K.n  ER,ISCREEN,IOUT 

HEAVE  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(ZQ=  -1.350E-1,  ZQDOT=-6.810E-3,  ZW=  -3.020E-1, 

&  ZWDOT=-2.430E- 1,  ZDB=  -2.580E-2,  ZDS=  -7.255E-2, 

&  CDZ=  1.0) 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

PA RAMETER(MQ=  -6.860E-2,  MQDOT=-  1.680E-2,  MW=  9.860E-2, 
&  MWDOT=-(i.81()E-3,  MDB=  6.940E-3,  MDS=  -4.120E-2) 

VEHICLE  CHARACTERISTICS 

PARAMETER(BOY  =  12()00.,  WT=12000„  L=17.425,  XB=0.0,  XG=0.0, 

&  ZB=0.0,  ZG=0.20,  IY=10000.,  RHO=1.940,  G=32.2. 

&  NU=8.47E-4) 

SURGE  CHARACTERISTICS 

PAR AMETER(UM AX=  6.0,  XUDOT=-7.580E-3,  XWDB=  9.660E- 3, 
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XWDS=  4.600E-2,  XQDB=-2.600E-3,  XQDS=  2.610E-2, 
XWW=  1.710E-1,  XQO=-  1.470E-2,  XWQ=-1.920E-1, 
XDBDB=-8.070E-3,  XDSDS=-  1.160E-2) 


PLANE  CHARACTERISTICS 
PARAMETER(DMAX=0.4) 

THRUSTER  CHARACTERISTICS 

PARAMETER(IVMAX=20.0,  XBV=6.70,  XSV=6.70) 

PROGRAM  VARIABLES 

PARAMETER(DELT=0.01,  PIE=3. 141592654,  ITER=20000) 

DEFINE  LENGTH  X  AND  BEAM  B  TERMS  FOR  THE  INTEGRATION 

X(l)=- 105.9/12. 

X(2)=- 104.3/ 12. 

X(3)=-99.3/12. 

X(4)=-94.3/12. 

X(5)=-87.3/12. 

X(6)=-  76.8/12. 

X(7)=-66.3/12. 

X(8)=-55.8/ 12. 

X(9)=72.7/ 12. 

X(10)-79.2/ 12. 

X(1 1)=83.2/12. 

X(12)=87.2/12. 

X(13)=91.2/12. 

X(14)=95.2/12. 

X(15)=99.2/12. 

X(16)=101 .2/ 12. 

X( 1 7)= 102. 1/12. 

X(I8)=  103.2/12. 

B(l)=75. 70/12. 

B(2)=75. 70/12. 

B(3)=75. 70/12. 

B(4)=75. 70/12. 

B(5)=75 .70/ 12. 

B(6)=75.70/12. 

B(7)=75. 70/12. 

B(8)=75.70/ 12. 

B(9)=75.70/ 12. 

B(10)=70. 24/12. 

B(  1 1 )=64.40/ 12. 

B(12)=58. 16/12. 

B(  131=49.84/ 12. 

B(  14)=38.88/ 12. 
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B(15)=26. 32/12. 

B(16)=18. 56/12. 

B(17)=12. 64/12. 

B(18)=0. 00/12. 

DATA  ENTRY 

WRITER,  *) 

WRITER, *)  ’ENTER  ORDERED  SPEED  IN  FT/SEC,  UD 
READ(V)  UD 

IF  (UD  .LT.  0.0  .OR.  UD  .GT.  UMAX)  THEN 
WRITE(V) 

WRITE(V)  ’UD  MUST  BE:  0.0  <  UD  <  6.0’ 
WRITE(V)  RE-ENTER  REALISTIC  SPEED.’ 
WRITER,  *) 

GO  TO  10 
ENDIF 

WRITE(V)  ’ENTER  ORDERED  DEPTH  IN  FEET,  ZD  = 
READ(V)  ZD 
IF  (ZD  .LT.  0.0)  THEN 
WRITE(V) 

WRITE(V)  ’ZD  MUST  BE:  ZD  >0.0’ 

WRITE(V)  RE-ENTER  REALISTIC  DEPTH’ 
WRITE(V) 

GO  TO  20 
ENDIF 

WRITE(V)  ENTER  SI  =’ 

READ(V)  SI 

WRITE(V)  ENTER  EITA  =  ’ 

READ(V)  EITA 
IF  (EITA  .LE.  0.0)  THEN 
WRITE(V) 

WRITE(V)  ’EITA  MUST  BE:  EITA  >  0.0’ 
WRITE(V)  ’RE-ENTER  REALISTIC  EITA’ 
WRITE(V) 

GO  TO  40 
ENDIF 

CALCULATE  THE  MASS 
M=WT/G 

SET  INITIAL  VALUES 

U=UD 

N=UD/0.012 

DB=0.0 

DS=0.0 

IBV=0.0 

ISV=0.0 
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W=0.0 
WH  AT=0.0 
Z1=0.0 
0=0.0 
THE=0.0 
Z=0.0 
WDOT=0.0 
ODOT=0.0 
UDOT=0.0 
THEDOT=0.0 
ZDOT=0.0 
TIME=0.0 
C 

C  CALCULATE  THE  MASS  MATRIX 
C 

MM(1,1)=M  -(RHO/2.)*L*L*L*ZWDOT 
MM(l,2)=-(M*XG+(RHO/2.)*L*L*L*L*ZQDOT) 

MM(1,3)=0.0 

MM(2,l)=-(M*XG+(RHO/2.)*L*L*L*L*MWDOT) 

MM(2,2)=IY-(RHO/2.)*L*L*L*L*L*MQDOT 

MM(2,3)=M*ZG 

MM(3,1)=0.0 

MM(3,2)=M*ZG 

MM(3,3)=M-0.5*RHO*L*L*L*XUDOT 

C 

C  INVERT  THE  MASS  MATRIX 

C 

CALL  LINRG(3,MM,3,MMINV,3) 

C 

C  INITIALIZE  COUNTERS 
C 

ISCREEN=1 

IOUT=l 

C 

C  OPEN  OUTPUT  FILE 

C 

OPEN(UNIT=15,FILE=’ALLOUT.DAT’,STATUS=’NEW’) 

WRITE(15,*)  TIME,Zb,Z,DB,DS,IBV,ISV,W,WHAT,Q,THE*180./PIE,UD,U,N 
C 

C  SIMULATION  BEGINS 

C 

DO  1000  1=1,  ITER 

C 

C  CALCULATE  THE  DRAG  FORCES 

C 

DO  50  K=l,18 

UCF=(W-0*X(K)) 

SGN-1.0 

IF  (UCF  ,LT.  0.0)  SGN=- 1.0 
VEC1(K)=B(K)*UCF*UCF*SGN 
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C 

c 

c 

& 

& 

& 

& 

& 

c 

c 

c 


c 

c 

c 


& 

& 

& 

& 

& 

& 

& 


& 

& 

& 

& 

& 

& 

& 


c 

c 

c 


VEC2(K)=B(K)*UCF*UCF*SGN*X(K) 

CONTINUE 

CALL  TRAP(18, VEC1  ,X,DRAGDX) 

CALL  TRAP(18,VEC2,X,DRAGXDX) 

RHS  OF  HEAVE  EQUATION 

F{1)  =  (0.5*RHO*L*L*ZW*U)*W  +  (M*U+0.5*RHO*L*L*L*ZQ*U)*Q 
+(M*ZG)*Q*Q  -  (0.5*RHO*CDZ)*DRAGDX 
+(WT-BOY)*COS(THE)  +  (0.5*RHO*L*L*U*U*ZDB)*DB 
+(0.5*RHO*L*L*U*U*ZDS)*DS 
+(0.25-0.00833333*U)*IBV 
+(0.25-0.00833333*U)*ISV 

RHS  OF  PITCH  EQUATION 

F(2)  =  (0.5*RHO*L*L*L*MW*U)*W 

+(0.5*RHO*L*L*L*L*MQ*U-M*XG*U)*Q  -  (M*ZG)*W*Q 

+(0.5*RHO*CDZ)*DRAGXDX  -  (XG*WT-XB*BOY)*COS(THE) 

-(ZG*WT-ZB*BOY)*SIN(THE) 

+(0.5*RHO*L*L*L*U*U*MDB)*DB 

+(0.5*RHO*L*L*L*U*U*MDS)*DS 

-XBV  *(0.25- 0.0(1^33333*  U)*IBV 

+XSV  *(0.25- 0.00833333  *U)*ISV 

RHS  OF  SURGE  EQUATION 

SIGNU=1.0 

IF  (  (U  .LT.  0.0)  .AND.  (N  .GT.  0.0)  )  SIGNU=-1.0 
IF  (  (U  .GT.  0.0)  .AND.  (N  .LT.  0.0)  )  SIGNU=-1.0 
RE=U*L/NU 

CD0=0.00385  +  1.296E- 17*(RE-  1.2E7)*(RE-1.2E7) 

F(3)  =  0.5*RHO*L*L*U*W*(XWDB*DB+XWDS*DS) 
+0.5*RHO*L*L*L*U*Q*(XQDB*DB+XQDS*DS) 
+0.5*RHO*L*L*XWW*W*W 
+(M*XG+0.5*RHO*L*L*L*L*XQQ)*Q*Q 
+(0.5*RHO*L*L*L*XWQ-M)*W*Q 
+0.5*RHO*L*L*U*U*(XDBDB*DB*DB+XDSDS*DS*DS) 
(WT-BOY)*SIN(THE) 

+0.5*RHO*L*L*CD0*(SIGNU*0.012*0.012*N*N-  U*U) 

WDOT  =  MMINV(1,1)*F(1)+MMINV(1,2)*F(2)+MMINV(1,3)*F(3) 
QDOT  =  MMINV(2,1)*F(1)+MMINV(2,2)*F(2)+MMINV(2,3)*F(3) 
UDOT  =  MMINV(3,1)*F(1)+MMINV(3,2)*F(2)+MMINV(3,3)*F(3) 
THEDOT  =  Q 

ZDOT  =  -  U*SIN(THE)  +  W*COS(THE) 

FIRST  ORDER  INTEGRATION 

W  =  W  +  DELT*WDOT 
Q  =  Q  +  DELT*QDOT 
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U  =  U  +  DELT’UDOT 
THE  =  THE  +  DELT*THEDOT 
Z  =  Z  +  DELT’ZDOT 
IF  (Z  .LT.  0.0)  Z=0.0 


CALCULATE  THE  CONTROL  LAW 

IF  (U  .LE.  0.75)  THEN 
DB=0.0 
DS=0.0 

CALL  OBSERVl(I,U,Q,THE,Z,ZD,IBV,ISV,DELT,WHAT) 
S1=1.0*WHAT  +  0.0*Q  -  0.1452*THE  +  0.4471*(Z-ZD) 
S2=0.0*WHAT  +  1.0*0  +  0.7074*THE  -  0.0007*(Z-ZD) 

IF  (ABS(Sl)  .LT.  SI)  SAT1=S1/SI 
IF  (SI  .LE.  -SI)  SAT1=- 1.0 
IF  (SI  .GE.  SI)  SAT1=1.0 
IF  (ABS(S2)  .LT.  SI)  SAT2=S2/SI 
IF  (S2  .LE.  -SI)  SAT2=- 1.0 
IF  (S2  .GE.  SI)  SAT2=1.0 

IBV  =  -  1.2666E3*WHAT  +  6.8616E3*Q  -  0.2037E3*THE 
&  -0.3097E4*EITA*SAT1  +  0.9703E4*EITA*SAT2 

ISV  =  -  1.5299E3*WHAT  -  7.1672E3*0  +  1.3030E3*THE 
&  -0.3464E4*EITA*SAT1  -  1.2169E4*EITA*SAT2 

ELSEIF  (  (U  .GT.  0.75)  .AND.  (U  .LE.  1.75))  THEN 

CALL  OBSERV2(I,U,Q,THE,Z,ZD,DB,IBV,DELT,WHAT) 
S1=1.0*WHAT  +  0.0*Q  -  0.0735*THE  +  0.0678*(Z-ZD) 
S2=0.0*WHAT  +  1.0*0  +  1.0855*THE  -  0.0735*(Z-ZD) 

IF  (ABS(Sl)  .LT.  SI)  SATI=S1/SI 
IF  (SI  .LE.  -SI)  SAT1=- 1.0 
IF  (SI  .GE.  SI)  SAT1  =  1.0 
IF  (ABS(S2)  .LT.  SI)  SAT2=S2/SI 
IF  (S2  .LE.  -SI)  SAT2=- 1.0 
IF  (S2  .GE.  SI)  SAT2=1.0 

DB  =  0. 005 1E3* WHAT  -  0.0818E3*Q  -  0.0023E3*THE 
&  -0.0016E3*EITA*SAT1  -  0.0937E3*EITA*SAT2 

IBV  =  -0.1347E3*WHAT  +  3.3747E3*Q  +  0.2712E3*THE 
&  -3.3095E3*EITA*SAT1  +  2.9370E3*EITA*SAT2 

DS=-DB 

IF  (U  .LT.  1.25)  DS=0.0 
ISV=IBV 

IF  (U  .GT.  1.25)  ISV=0.0 

ELSE 

IBV=0.0 

ISV=0.0 

CALL  OBSERV3(I,U,Q,THE,Z,ZD,DB,DS,DELT,WHAT) 
S1  =  1.0*WHAT  +  0.0*0  -  0.0945*THE  +  0.0328*(Z-ZD) 
S2=0.0*WHAT  +  1.0*Q  +  1.3127*THE  -  0.0945*(Z-ZD) 

IF  (ABS(Sl)  .LT.  SI)  SAT1=S1/SI 
IF  (SI  .LE.  -SI)  SAT1=- 1 .0 
IF  (SI  .GE.  SI)  SAT1=1.0 
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IF  (ABS(S2)  .LT.  SI)  SAT2=S2/SI 
IF  (S2  .LE.  -SI)  SAT2=- 1.0 
IF  (S2  .GE.  SI)  SAT2=1.0 

DB  =  -  1.2124*WHAT  -  17.7554*0  -  6.3692*THE 
&  +9.2671  *EITA*SAT1  -  18.1220*EITA*SAT2 

DS  =  -0.6568*WHAT  +  4.4622*Q  +  2.3181  *THE 
&  +1.7528*EITA*SAT1  +  8.3426*E1TA*SAT2 

ENDIF 

SPEED  CONTROLLER 
S3=U-UD 

IF  (ABS(S3)  .LT.  SI)  SAT3=S3/SI 
IF  (S3  .LE.  -SI)  SAT3=- 1.0 
IF  (S3  .GE.  SI)  SAT3=1.0 
N=- 1 153.9*SAT3  +  83.33*U 

CHECK  FOR  SATURATION 

IF  (DB  .GT.  DMAX)  DB=DMAX 
IF  (DB  .LT.  -DMAX)  DB=-DMAX 
IF  (DS  .GT.  DMAX)  DS=DMAX 
IF  (DS  LT.  -DMAX)  DS=-DMAX 
IF  (IBV  GT.  IVMAX)  IBV=IVMAX 
IF  (IBV  LT.  -IVMAX)  IBV=-IVMAX 
IF  (ISV  GT.  IVMAX)  ISV=IVMAX 
IF  (ISV  LT.  -IVMAX)  ISV=-IVMAX 
IF  (N  .GT.  500.)  N=500. 

IF  (N  .LT.  -500.)  N=-500. 

PRINT  TO  SCREEN 

IF  (ISCREEN  .EO.  100)  THEN 
TIME=I/100. 

WRITE(*,900)  TIME, ZD, Z,W, WHAT 
i  FORM AT(’  TIME=  ’,F6.2,1X,’ZD=  \F6.2,1X,’Z=  ’, 

&  F8.4,1X,’W=  ’,F7.4,1X,’WHAT=  \F14.7) 

ISCREEN=0 
ENDIF 

PRINT  TO  FILE 

IF  (IOUT  .EQ.  10)  THEN 
TIME=I/100. 

WRITE(15,*)  TIME,ZD,Z,DB,DS,IBV,ISV,W,WHAT,0, 
&  THE*180./PIE,UD,U,N 

IOUT=0 
ENDIF 

ISCREEN=ISCREEN+1 

IOUT=IOUT+l 
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1000  CONTINUE 

CLOSE(UNIT=15) 

STOP 

END 

C 

C  NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 
C 

SUBROUTINE  TRAP(N,A,B,OUT) 

INTEGER  N,N  1,1 

REAL  A(N),B(N),OUT,OUTl 

N1=N-1 

OUT=0.0 

DO  10  1=1, N1 

OUT1  =  0.5*(A(I)  +  A(I+1))  *  (B(I+1)  -  B(I)) 

OUT  =  OUT  +  OUT1 
10  CONTINUE 
RETURN 
END 
C 

C  OBSERVER  FOR  THE  SPEED  RANGE:  0.00  <  U  <  0.75 
C 

SUBROUTINE  OBSERV1(I,U,0,THE,Z,ZD,IB V,ISV,DELT,WHAT) 
INTEGER  I 

REAL  U,0,THE,Z,ZD,IBV,ISV,DELT,WHAT 

REAL  Zl,ZlDOT 

COMMON  Z1 

REAL  L1,L2,L3 

REAL  F 

REAL  G1,G2,G3 
REAL  H1,H2 

REAL  A 1 1,A12,A13,B1 1,B12,A21,A22,A23,B21,B22,A32,A41,A43 
REAL  SI 

PARAMETER(Sl=-0.9) 

A  11=- 0.060554828  *U 
A 1 2= -0. 134756503  *U 
A13=  0.02510025 

B1 1=  6.913620E-04*(0.25000000-8.33333333E-03*U) 

B12=  5.512506E-04*(0.25000000-8.33333333E-03*U) 

A21=  0.015006049*U 
A 22=- 0. 167296702 *U 
A23=- 0.06676024 

B21  =  -  1.968203E-04*(0. 25000000- 8. 33333333E- 03  *U) 

B22=  1 . 759 190E -04  *(0.25000000- 8. 33333333E-03*U) 

A32=  1.0 
A41=  1.0 
A43=- U 

L1=(A11  -S1)/(A21+A41) 

L2=0.0 

L3=L1 

F=A11  -  Ll*A21  -  L3*A41 
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H1=B11-L1*B21 

H2=B12- LI *B22 

G1=A12  -  L1*A22  -  L2*A32+F*L1 

G2=A13  -  L1*A23  -  L3*A43+F*L2 

G3=F*L3 

ZlDOT=F*Zl+Gl  *Q+G2*THE+G3*(Z-ZD)+H1  *IBV+H2*ISV 
Zl=Zl+ZlDOT*DELT 
WHAT=L1*Q+L2*THE+L3*(Z-ZD)+Z1 
IF  (I  .LE.  1000)  WHAT=0.0 
RETURN 
END 
C 

C  OBSERVER  FOR  THE  SPEED  RANGE.  0.75  <  U  <  1.75 
C 

SUBROUTINE  OBSERV2(I,U,Q,THE!Z,ZD,DB,IBV,DELT,WHAT) 
INTEGER  I 

REAL  U,Q,THE,Z,ZD,DB,IBV,DELT,WHAT 

REAL  Zl,ZlDOT 

COMMON  Z1 

REAL  L1,L2,L3 

REAL  F 

REAL  G1,G2,G3 
REAL  H1,H2 

REAL  All,A12,A13,Bn,B12,A21,A22,A23,B21,B22,A32,A41,A43 
REAL  SI 

PARAMETER(Sl  =  -0.32) 

A  11  =  - 0.060554828*  U 
A 1 2=  -  0.1 34756503  *U 
A13=  0.02510025 
B1 1  =  -0. 005093606  *U*U 

B 12=  6. 9 13620E- 04  *(0.25000000  -  8.33333333E-03*U) 

A21=  0.015006049*U 
A  22= -0.1 67296702  *U 
A23=- 0.06676024 
B21=  0.00 1070202  *U*U 

B22=-  1.968203E  -  04*(0. 25000000- 8. 33333333E-03*U) 

A32=  1.0 
A41=  1.0 
A43=-U 

Ll=(iA  1 1  -S1)/(A21+A41) 

L2=0.0 

L3=L1 

F=A1 1  -  L1*A21-L3*A41 

H1=B11-L1*B21 

H2=B12- LI *B22 

G1=A12  -  LI  *A22  -  L2*A32+F*L1 

G2=A13  -  Ll  *A23  -  L3*A43+F*L2 

G3=F*L3 

ZlDOT=F*Zl  +G1  *Q+G2*THE+G3*(Z-ZD)+H1*DB+H2*IBV 
Zl=Zl+ZlDOT*DELT 


171 


n  n  n 


WHAT=L1*Q+L2*THE+L3*(Z-ZD)+Z1 
IF  (I  .LE.  1000)  WHAT=0.0 
RETURN 
END 

OBSERVER  FOR  THE  SPEED  RANGE:  U  >  1.75 

SUBROUTINE  OBSERV3(I,U,Q,THE,Z,ZD,DB,DS,DELT,WHAT) 
INTEGER  I 

REAL  U,Q,THE,Z,ZD,DB,DS,DELT,WHAT 

REAL  Z1.Z1DOT 

COMMON  Z1 

REAL  L1,L2,L3 

REAL  F 

REAL  G1,G2,G3 
REAL  H1.H2 

REAL  A11,A12,A13,B11,B12,A21,A22,A23,B21,B22,A32,A41,A43 
REAL  SI 

PARAMETER(Sl=-0.88) 

A 1 1  =  - 0.060554828  *U 
A 12= -0.1 34756503  *U 
A13=  0.02510025 
B1 1=- 0.005093606  *U*U 
B 12= -0.01 10645 14* U  *U 
A21=  0.015006049*U 
A 22= -0.1 67296702 *U 
A23=- 0.06676024 
B21=  0.00 1070202*  U*U 
B22=- 0.0056581 07*U*U 
A32=  1.0 
A41=  1.0 
A43=- U 

Ll=(Al  1  -  SI  )/(A21+A41 ) 

L2=0.0 

L3=L1 

F=A1 1-L1*A21-L3*A41 

H1=B11-L1*B21 

H2=B12-L1*B22 

G1=A12-  LI  *A22-  L2*A32+F*L1 

G2=A13  -  L1*A23  -  L3*A43+F*L2 

G3=F*L3 

ZlDOT=F*Zl+Gl  *Q+G2*THE+G3*(Z-ZD)+H1  *DB+H2*DS 

Zl=Zl+ZlDOT*DELT 

WHAT=L1  *Q+L2*THE+L3*(Z-  ZD)+Zl 

IF  (I  .LE.  1000)  WHAT=0.0 

RETURN 

END 
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APPENDIX  D.  AUV  STEERING  SIMULATION  PROGRAM;  MIMO  SLIDING 
MODE  LOS  CONTROLLER  DESIGNED  BY  LQR  METHOD; 
DISTURBANCE  REJECTION;  V,  U0  AND  Vc  OBSERVED 


PROGRAM  SD V LOS R R300OBS_LQR 

^t********************^********************************************************** 

C 

C  PROGRAM  SDVLOSRR300OBS_LQR.FOR 
C 

C  WRITTEN  BY  LT  TODD  D.  HAWKINSON,  USN 
C 

C  FOR  AUV  THESIS  WORK 
C 

C  THIS  PROGRAM  CONTROLS  A  2  RUDDER  SDV  WITH  A  SLIDING  MODE 
C  LINE-OF-SIGHT  CONTROLLER  FOR  STEERING. 

C 

C  SPEED  IS  ALSO  CONTROLLED  USING  A  SLIDING  MODE  CONTROLLER 
C  DEVELOPED  BY  LIENARD. 

C 

C  THE  EQUATIONS  HAVE  BEEN  LINEARIZED  AT  U  =  3.0  FT/S  TO  OBTAIN 
C  THE  SLIDING  MODE  CONTROL  LAWS. 

C 

C  THE  PROGRAM  HAS  OBSERVERS  FOR  V,  VC,  AND  UC. 

f' 

v_. 

C  THE  OUTPUT  OF  THE  PROGRAM  IS  WRITTEN  TO  FILES,  LOSCURR.DAT, 

C  LOSWAYPT.DAT,  LOSALLOUT.DAT.  THESE  FILES  ARE  THEN  ACCESSED 
C  BY  THE  DISSPLA  PLOTTING  PROGRAM,  PLOT8.FOR,  TO  GENERATE 
C  OUTPUT  GRAPHICS. 

C 

C  THIS  PROGRAM  UTILIZES  SUBROUTINES  FROM  THE  IMSL  MATH 
C  LIBRARY,  VERSION  1.1,  (COPYRIGHT  JANUARY  1989  BY  IMSL,  INC.). 

C 

**•*******»*«*•****•****»*•«***»**»***»**«***»***«****»******»*»**»*»*********** 

C 

C  YAW  CHARACTERISTICS 
C 

REAL  NR,NRDOT,NV,NVDOT,NDBR,NDSR,CDY,R,RDOT,PSI,PSIDOT, 

&  PSIDX.PSIX 

SWAY  CHARACTERISTICS 

REAL  YR,YRDOT,YV,YVDOT,V,VDOT,YDBR,YDSR 

SURGE  CHARACTERISTICS 
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REAL  U,UMAX,UD,N,RE,ETA,CDO,XPROP,XUDOT,XVR,XRR,XRDBR, 

&  X  RDSR,X  VV,XVDBR,X  VDSR,XDBRDB  R,XDSRDSR, SIGN  U,U  DOT 

RUDDER  CHARACTERISTICS 

REAL  DMAX,DBR,DSR 

SLIDING  MODE  CONTROL  VARIABLES 

REAL  SI,EIT A, SSI ,SS2,SAT  1  ,SAT2,SS3,SAT3 

VEHICLE  CHARACTERISTICS 

REAL  WT,L,XG,YG,IZ,RHO,G,M,X(18),H(18),NU 

NAVIGATOR  VARIABLES 

REAL  UCO,UC,VCO,VC 
REAL  TARGET, PSID,DAWAY, ALPHA 

REAL  XD,XDl,XD2,XPOS,XCURR.XCTE,YD,YDl,YD2,YPOS,YCURR,YCTE 
REAL  X DOT, Y DOT 
INTEGER  IWAY.INAV 

OBSERVER  VARIABLES 

REAL  A  11,A12,A21.A22,B11,B12,B21,B22,L12,L23,L34,VHAT,UCHAT. 

&  VCHAT,UCOHAT,VCOHAT,Zl,Z2,Z3,ZlDOT,Z2DOT,Z3DOT,Sl.S2,S3 

PROGRAM  VARIABLES 

REAL  MM(3,3),MMINV(3,3),F(3),OUT(3) 

REAL  DELT 
REAL  PIE 
REAL  SGN 

REAL  VEC1(18),VEC2(18) 

REAL  DRAGDX,DRAGXDX,UCF 
REAL  TIME 

INTEGER  I,ITER,ISCREEN, K,IOUT 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(NR=  -  1.640E-2,  NRDOT=-3.400E-3,  NV=  -7.420E-3, 

&  NVDOT=  1.240E-3,  NDBR=  1.290E-2,  NDSR=  -1.290E-2, 

&  CDY=  3.500E- 1) 

C 

C  SWAY  HYDRODYNAMIC  COEFFICIENTS 
C 

PARAMETER(YR=  2.970E-2,  YRDOT=  1.240E-3,  YV=  -9.310E-2, 

&  YVDOT=-5.550E-2,  YDBR=  2.730E-2,  YDSR=  2.730E-2) 

C 
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SURGE  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(UM AX=  6.0  ,  XUDOT=  -7.580E-3,  XVR=  1.890E-2, 

&  XRR=  4.010E-3,  X RDBR=  8.IS0E-4,  XRDSR=-8.180E-4, 

&  XVV=  5.290E-2,  XVDBR=  1.730E-3,  XVDSR=  1.730E-3, 

&  XDBRDBR=-1.010E-2,  XDSRDSR=-  1.010E-2) 

RUDDER  CHARACTERISTICS 

PARAMETER(DMAX=0.4) 

VEHICLE  CHARACTERISTICS 


PA RAMETER(WT= 12000.,  L=17.425,  XG=0.0,  YG=u.C,  IZ=10000„ 
&  RHO=l .940,  G=32.2,  NU=8.47E-4) 

NAVIGATOR  VARIABLES 

PARAMETER(TARGET=8.71250) 

OBSERVER  VARIABLES 

PARAMETER(A1 1=-0. 04217^797,  ,U2— 0.351578237, 

&  A21  =  - 0.002794897,  A22=-0.09841587C, 

&  B 1 1  —  0.012974539,  B12=  0.0"  1513052, 

&  B21  =  0.004421618,  B22=-0.004244119, 

&  Sl=-1.0,  S2=  -1.1, 

&  S3=  - 1.2) 

PROGRAM  VARIABLES 


PARAMETER(PIE=3.141593,DF.LT=0.01,ITER=50000) 


C 


DFFINE  LENGTH  X  AND  HEIGHT  TERMS  FOR  THE  INTEGRATION 


X(  1)=- 105.9/12 
X(2)=- 104.3/12 
X(3)=-99.3/ 12. 
X(4)=-94.3/ 12. 
X(5  )=  -  87.3/ 12. 
X(6)=-  76.8/ 12. 
X(7)=-66.3/12. 
X(8)=-55.8/ 12. 
X(9)-72.7/12. 
X(10)=7°.2/12. 
X(  1 1  )=83.2  / 12. 
X  ( 1 2)=87.2/ 12 
X(13)=91.2/12. 
X(14)-95.2/12. 
X(15)=99.2/12. 
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X(16)=101.2/12. 

X(17)=102.1/12. 

X(18)=103.2/12. 

H(l)=0.0/12. 

H(2)=2. 28/12. 

H(3)=8. 24/12. 

H(4)=13.96/12. 

H(5)=19. 76/12. 

H(6)=25. 10/12. 

H(7)=29. 36/12. 

H(8)=31. 85/12. 

H(9)=31. 85/12. 

H(10)=30.00/ 12. 

H(1 1)=27. 84/12. 

H(12)=25. 12/12. 

H(13)=21.44/12. 

H(14)=17. 12/12. 

H(15)=12.00/12. 

H(16)=9. 12/12. 

H(17)=6. 72/12. 

H(18)=0.00/12. 

data  entry 

WRITE(*,*) 

10  WRITER, *)  ENTEk  ORDERED  SPEED  IN  FT/SEC,  UD  = 

READ(V)  UD 

IF  (UD  .LT.  0.0  .OR.  UD  .GT.  UMAX)  THEN 
WRITE(V) 

WRITE(V)  ’  UD  MUST  BE:  0.0  <=  UD  <=  6.0’ 
WRITE(V)  ’  RE-ENTER  REALISTIC  SPEED  .  .  .’ 
WRITE(V) 

GO  TO  10 
ENDIF 
WRITE(V) 

WRITER, *)  ENTER  GLOBAL  CURRENTS,  UCO.VCO  =  ? 
READ(V)  UCO,VCO 

OPEN(UNIT=19,FILE=’LOSCURR.DAT’,STATUS=’NEW’) 
WRITE(19,25)  UCO,VCO 
25  FORMAT(F6.3,lX,F6.3) 

CLOSE(UNIT=19) 

WRITE(*,*) 

30  WRITE(*,*)  ’ENTER  CONTROLLER  SI  =’ 

READ(*,*)  SI 
WRITE(V) 

40  WRITE(V)  ENTER  CONTROLLER  EITA  =  ’ 

READ(V)  EITA 
IF  (EITA  ,LE.  0.0)  THEN 
WRITE(V) 

WRITE(V)  ’EITA  MUST  BE:  EITA  >  0.0’ 
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IITE(V)  ’RE-ENTER  REALISTIC  EITA’ 

WRITE(*,*) 

GO  TO  40 
ENDIF 
WRITE(*,*) 

WRITE(*,*)  ’INITIAL  HEADING  IS  000  DEGREES  (DUE  NORTH)’ 
WRITE(V)  ’INITIAL  POSITION  IS  (0.0,0.0)’ 

WRITE(V)  'ENTER  FIRST  WAY-POINT:  X,Y  =  ?’ 

READ(V)  XD.YD 

SET  INITIAL  STARTING  POSITION 


XD1=0.0 

YD1=0.0 

XD2=XD 

YD2=YD 

OPEN(UNIT=20,FILE=’LOSWAYPT.DAT’,STATUS=’NEW’) 
WRITE(20,*)  XD1.YD1 
WRITE(20,*)  XD2.YD2 

CALCULATE  THE  MASS 


M=WT/G 


SET  INITIAL  VALUES 

ALPHA=ATAN2((YD2- YDl),(XD2-  XD1)) 

UC=UCO*COS(ALPHA)+VCO*SIN(ALPHA) 

VC=VCO*COS(ALPHA)-UCO*SIN(ALPHA) 

U=UD 

N=UD/0.012 

DBR=0.0 

DSR=0.0 

V=0.0 

R=0.0 

PSI=0.0 

PSID=0.0 

XPOS=0.0 

YPOS=0.0 

XCURR=XPOS 

YCURR=YPOS 

XCTE=0.0 

YCTE=0.0 

TIME=0.0 

VHAT=0.0 

UCHAT=0.0 

VCHAT=0.0 

UCOHAT=0.0 

VCOHAT=0.0 

Z1=0.0 
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Z2=0.0 

Z3=0.0 

Z1DOT=0.0 

Z2DOT=0.0 

Z3DOT=0.0 

CALCULATE  THE  MASS  MATRIX 

MM(1,1)=(M  -0.5*RHO*L*L*L*XUDOT) 

MM(I,2)=0.0 

MM(1,3)=-M*YG 

MM(2,1)=0.0 

MM(2,2)=(M  -  0.5*RHO*L*L*L*  YVDOT) 
MM(2,3)=(M*XG-0.5*RHO*L*L*L*L*YRDOT) 

MM(3,1)=-M*YG 

MM(3,2)=(M*XG-0.5*RHO*L*L*L*L*NVDOT) 

MM(3,3)=(IZ-0.5*RHO*L*L*L*L*L*NRL'OT) 

INVERT  THE  MASS  MATRIX  USING  IMSL  LIBRARY  SUBROUTINE 

CALL  LINRG(3,MM,3,MMINV,3) 

INITIALIZE  THE  COUNTERS 

ISCREEN=1 
INA  V=0 
IWAY=1 
IOUT=I 

OPEN  OUTPUT  FILES 

OPEN(UNIT=18,FILE=’LOSALLOUT.DAT,,STATUS=’NEW’) 
WRITE(18,*)  TIME,XPOS,YPOS,PSID,PSI,DBR,DSR,UD,U,V,VHAT,R, 
&  UC,UCHAT,VC,VCHAT 

SIMULATION  BEGINS 

DO  1000  1=1, ITER 

CALCULATE  THE  DRAG  FORCES 

DO  50  K=l,18 

UCF=(V+R*X(K)) 

SGN=1.0 

IF  (UCF  .LT.  0.0)  SGN=- 1.0 

VEC1(K)=H(K)*UCF*UCF*SGN 

VEC2(K)=H(K)*UCF*UCF*SGN*X(K) 

CONTINUE 

Cr»LL  TRAP(18, VECT.X.DRAGDX) 

CALL  TRAr(18,VEC2,X,DRAGXDX) 


178 


c 

C  RHS  OF  SURGE  EQUATION 

C 

SIGNU=1.0 

IF  (  (U  .LT.  0.0)  .AND.  (N  .GT.  0.0)  )  SIGNU=-1.0 
IF  (  (U  .GT.  0.0)  .AND.  (N  .LT.  0.0)  )  SIGNU=-1.0 
RE=U  *L/NU 

CD0=0.00385  +  1.296E-  17*(RE-  1.2E7)*(RE-  1.2E7) 

F(1)=0.5*RHO*L*L*U*  V*(X  VDBR*DBR  +  XVDSR’DSR) 

&  +  0.5*RHO*L*L*L*U*R*(XRDBR*DBR  +  XRDSR*DSR) 

&  +  0.5*RHO*L*L*XVV*V*V 

&  +  (M*XG  +  0.5*RHO*L*L*L*L*XRR)*R*R 

&.  +  (M  +  0.5*RHO*L*L*L*X  VR)*V*R 

&  +  0.5*RHO*L*L*U*U*(XDBRDBR*DBR*DBR  +  XDSRDSR*DSR*DSR) 

&  +  0.5*RHO‘L*L*CD0*(SIGNU*0.012*0.012*N*N  -  U*U) 

C 

C  RHS  OF  SWAY  EQUATION 

C 

F(2)=0.5*RHO*L*L*YV*U*  V 
&  +  (0.5*RHO*L*L*L*YR  -  M)*U*R 

&  +  M*YG*R*R 

&  +0  5*RHO*L*L*U*U*(YDBR*DBR  +  YDSR*DSR) 

&  -  0.5*RHO*CDY*DRAGDX 

C 

C  RHS  OF  YAW  EQUATION 

C 

F(3)=0.5*RHO*L*L*L*NV*U*V 
&  +  (0.5*RHO*L*L*L*L*NR*U  -  M*XG*U)*R 

&  -  M*YG*V*R 

&  +  0.5*RHO*L*L*L*U*U*(NDBR*DBR  +  NDSR*DSR) 

&  -  0.5*RHO*CDY*DRAGXDX 

C 

C  DEFINITION 

C 

PSIDOT=R 

C 

C  MULTIPLY  INVERTED  MASS  MATRIX  AND  F  VECTOR 

r 

CALL  MURRV(3,3,MMINV,3,3,F,1,3.0UT) 

UDOT=OUT(l) 

VDOT=OUT(2) 

RDOT=QUT(3) 

XDOT=UCO+U*COS(PSI)-V*SIN(PSI) 

Y  DOT = V  CO+U  *SI  N(PSI)+ V  *COS(PSI) 

C 

C  FIRST  ORDER  INTEGRATION 

C 

U=U  +  DELT’UDOT 
V=V  +  DELT*VDOT 
R=R  +  DELT*RDOT 
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PSl=PSI  +  DELT’PSIDOT 

MAKE  PSI  TO  BE:  -180  <  PSI  <=  180  DEGREES 

IF  (PSI  GT.  PIE)  THEN 
PSI=PSI  -  2.0*PIE 
ELSEIF  (PSI  .LE.  -PIE)  THEN 
PSI=PSI  +  2.0*PIE 
ENDIF 

XPOS=XPOS+DELT*XDOT 
YPOS=YPOS+DELT*YDOT 

CHECK  IF  TIME  FOR  NAV  UPDATE 

INA  V=INAV+1 
IF  (IN AV  .NE.  1)  GO  TO  80 
INAV=0 
XCURR=XPOS 
YCURR=YPOS 

DAWAY=SQRT((XCURR  -  XD)**2  +  (YCURR- YD)**2) 

CHECK  DISTANCE  TO  WAY-POINT 

IF  (DAWAY  LT.  TARGET)  THEN 

MISS  DISTANCE  TO  SCREEN,  ENTER  NEW  WAY-POINT, 
CALCULATE  NEW  ALPHA, UCHAT,VCHAT,XCTE,YCTE,Z2,Z3 

WRITE(V) 

WRITE(*,60)  IWAY 
60  FORMATC  WAY-POINT  #\ll,’  REACHED.’) 

WRITE(*,65)  DAWAY 

65  FORMATf  DAWAY  =  ’,F8.3,’  FEET’) 

UCOHAT=UCHAT*COS(ALPHA)-VCHAT*SIN(ALPHA) 
VCOHAT=VCHAT*COS(ALPHA)+UCHAT*SIN(ALPHA) 
IWAY=IWAY+1 
WRITE(*,70)  IWAY 

70  FORMAT(’  ENTER  WAY-POINT  #’, II,’:  X,Y  =  ?’) 

READ(V)  XD,YD 

XD1=XD2 

YD1=YD2 

XD2=XD 

YD2=YD 

WRITE(20,*)  XD2,YD2 
ALPH  A=ATAN2((YD2- YD1),(XD2-XD1)) 
UC=UCO*COS(ALPHA)+VCO*SIN(ALPHA) 
VC=VCO*COS(ALPHA)-  UCO'SIN(ALPHA) 

UCH  AT=UCOHAT*COS(ALPHA)+VCOHAT*SIN(ALPHA) 
VCHAT=VCOHAT*COS(ALPHA)-UCOHAT*SIN(ALPHA) 
XCTE=(YCURR- YD1)*SIN(ALPHA) 
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&  +(XCURR  -XDl)*COS(ALPHA) 

YCTE-(YCURR- YD  l)*COS(  ALPHA) 

&  -(XCURR  -XD1)*SIN(ALPHA) 

Z2=VCHAT+S2*YCTE 

Z3=UCHAT+S3*XCTE+U*COS(PSI  -ALPHA) 

ENDIF 

C 

C  CALCULATE  NEW  PSID  DUE  TO  CHANGE  OF  XD,YD  AND/OR 

C  XCURR, YCURR 

C 

80  PSID=ATAN2((YD- YCURR), (XD-  XCURR)) 

XCTE=(YCURR- YD1)*SIN(ALPHA)+(XCURR-  XDl)*COS(ALPHA) 
YCTE=(YCURR-YDl)*COS(ALPHA)-(XCURR-XDl)*SIN(ALPHA) 

C 

C  CALCULATE  THE  QUICKEST  ROUTE  PSID 

C 

IF((PSI  .GE.  0.0  .AND.  PSI  .LE.  PIE)  .AND. 

&  (PSID  .GT.  -PIE  .AND.  PSID  .LT.  PSI-PIE))  THEN 

PS1D=PSID  +  2.0*PIE 

ELSEIF((PS!  GT.  -PIE  .AND.  PSI  .LT.  0.0)  .AND. 

&  (PSID  .GT.  PJl+PIE  .AND.  PSID  .LE.  PIE))  THEN 

PSID=PSID  -  2.0*PIE 
ENDIF 

C  OBSERVERS  FOR  SWAY  VELOCITY  AND  CURRENT  VELOCITIES 

£'*****«*****•***********,*«***•******•*,***************•*••***********•**•******* 


L12=(A1  I*U  -  S1)/(A2I*U) 

L23=-S2 

L34=-S3 

ZlDOT=Sl*Zl  +  (A12*U-  L12*A22*U+Sl  *L12)*R 
&  +  (Bll  -B21  *L12)*U*U*DBR  +  (B12-B22*L12)*U*U*DSR 

Z2DOT=S2*Zl  +  S2*Z2  -  L23*U*SIN(PSI- ALPHA)  +  S2*L12*R 
&  +  S2*L23*YCTE 

Z3DOT=S3*Z3  -  S3*S3*XCTE 
C 

C  FIRST  ORDER  INTEGRATION 

C 

Z1=Z1  +DELT*ZlDOT 
Z2=Z2+DELT*Z2DOT 
Z3=Z3+DELT*Z3DOT 

C 

C  FINAL  OBSERVER  EQUATIONS 

C 


VHAT=L12*R  +  Z1 
VCHAT=L23*YCTE  +  Z2 

UC HAT=Z3  -  S3*XCTE  -  U*COS(PSI- ALPHA) 


C  END  OBSERVERS 

C 


181 


CALCULATE  THE  CONTROL  LAWS 


IE  (VC/U  .GT.  1.0)  THEN 
ARG=1.0 

ELSEIF  (VC/U  .LT.  -1.0)  THEN 
ARG=- 1.0 

ELSE 

ARG=VC/U 

ENDIF 

SS1=1.0*V 

SS2=1.0*R  +  0.5*(PSI-PSID)  +  0.5*ASIN(ARG) 

SS3=U  -  UD 

IF  (ABS(SSl)  .LT.  SI)  SAT1=SS1/SI 
IF  (SSI  .LE.  -SI)  SAT1=- 1.0 
IF  (SSI  .GE.  SI)  SAT1=1.0 
IF  (ABS(SS2)  .LT.  SI)  SAT2=SS2/SI 
IF  (SS2  .LE.  -SI)  SAT2=- 1.0 
IF  (SS2  .GE.  SI)  SAT2=1.0 
IF  (ABS(SS3)  .LT.  1.0)  SAT3=SS3/1.0 
IF  (SS3  .LE.  -1.0)  SAT3=- 1.0 
IF  (SS^  .GE.  1.0)  SAT3=1.0 

DBR=  0.6642* VHAT  +  2.2219*R  -  EITA*4.4499*SAT1 
-  EITA*12.0713*SAT2 

DSR=  0.4725*VHAT  +  7.6752*R  -  EITA*4.6361  *SAT1 
+  EITA*  13.6037  *SAT2 
N=-  1153.9*SAT3  +  83.33*U 

CHECK  FOR  SATURATION 

IF  (DBR  .GT.  DMAX)  DBR=DMAX 
IF  (DBR  .LT.  -DMAX)  DBR=-DMAX 
IF  (DSR  GT.  DMAX)  DSR=DMAX 
IF  (DSR  LT.  -DMAX)  DSR=-DMAX 
IF  (N  .GE.  '00.0)  N=500.0 
IF  (N  .LE.  -500.0)  N=-500.0 

OUTPUT 

IF  (PSID  .LT.  0.0  .AND.  PSID  .GT.  -2.0*PIE)  THEN 
PSIDX=(PSID+2.0*PIE)*180.0/PIE 

ELSE 

PSIDX=PSID*180.0/PIE 

ENDIF 

IF  (PSI  .LT.  0.0  .AND.  PSI  .GT.  -PIE)  THEN 
PSIX=(PSI+2.0*  PIE)  *180.0 /PIE 

ELSE 

PSI  X=PSI*  180.0/ PIE 
ENDIF 

IF  (ISCREEN  .EQ.  200)  THEN 

WRITE(*,90)  PSIDX,PSIX,XPOS,YPOS 
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FORM AT(’  PSID=’,F9.4,2X,’PSI=’,F9.4,2X, 

&  XPOS=  ’F8.2,2X,’YPOS=  ’,F8.2) 

ISCREEN=0 
ENDIF 

IF  (IOUT  .EQ.  20)  THEN 
TIME=I./100. 

WRU  E(18,*)  TIME.XPOS,YPOS,PSIDX,PSIX,DBR,DSR, 

&  UD,U,V,VHAT,R,UC,UCHAT,VC,VCH  AT 

IOUT=0 
ENDIF 

IOUT=IOUT+l 
ISCREEN=ISCREEN  +  1 
1000  CONTINUE 

CLOSE(UNIT=18) 

CLOSE(UNIT=20) 

STOP 
END 

NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 

SUBROUTINE  TRAP(N,A,B,OUT) 

INTEGER  N,N1,I 
REAL  A(N),B(N),OUT,OUTl 
N1=N-1 
OUT=0.0 
DO  10  I=1,N1 

OUT1  =  0.5*(A(I)  +  A(I+1))  *  (B(I+1)  -  B(I» 

OUT  =  OUT  +  OUTl 
10  CONTINUE 
RETURN 
END 
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APPENDIX  E.  AUV  STEERING  SIMULATION  PROGRAM;  MIMO  SLIDING 
MODE  CTE  CONTROLLER  DESIGNED  BY  LQR  METHOD; 
DISTURBANCE  REJECTION;  V,  Uc,  AND  Vc  OBSERVED 


PROGRAM  SDVCTERR300OBS_LQR 

^'******4^*********+**********+**+************************##***+**********t****++* 

C 

C  PROGRAM  SD VOTER R300OBS _ LQR. FOR 

C 

C  WRITTEN  BY  LT  TODD  D.  HAWKINSON,  DSN 
C 

C  FOR  AUV  THESIS  WORK 

C 

C  THIS  PROGRAM  CONTROLS  A  2  RUDDER  SDV  WITH  A  SLIDING  MODE 
C  CROSS-TRACK-ERROR  CONTROLLER  FOR  STEERING. 

C 

c  SPEED  IS  ALSO  CONTROLLED  USING  A  SLIDING  MODE  CONTROLLER 
C  DEVELOPED  BY  LIENARD. 

C 

C  THE  EQUATIONS  HAVE  BEEN  LINEARIZFD  AT  U  =  3.0  FT/SEC  TO  OBTAIN 
C  THE  SLIDING  MODE  CONTROL  LAWS. 

C 

C  THE  OUTPUT  OF  THE  PROGRAM  IS  WRITTEN  TO  FILES,  CTECURR.DAT, 

C  CTEWAYPT.DAT,  LOSALLOUT.DAT.  THESE  FILES  ARE  THEN  ACCESSED 
C  BY  THE  DISSPLA  PLOTTING  PROGRAM  PLOTIO.FOR.  TO  GENERATE 
C  OUTPUT  GRAPHICS. 

C 

C  THIS  PROGRAM  UTILIZES  SUBROUTINES  FROM  THE  IMSL  MATH 
C  LIBRARY,  VERSION  1.1,  (COPYRIGHT  JANUARY  1989  BY  IMSL.  INC.). 

C 

C  THE  PROGRAM  HAS  3  OBSERVERS  FOR  V.  VC,  AND  1 1C. 

C 

^'*  +  +  +  +  +  +  +  4^**  +  *  +  **  +  +  +  +  +  +  +  «**  +  +  +  +  *^*  +  t******t*****  +  +  ***4>  +  *****  +  +  **  +  **  +  **  +  **4'  +  *  +  +  + 

C 

C  YAW  CHARACTERISTICS 
C 

REAL  NR.NRDOT.N V,NVDOT,NDBR,NDSR,CDY,R,RDOT,PSl,PSIDOT, 

&  PSIX 

SWAY  CHARACTERISTICS 

C 

REAL  YR, YRDOT,YV,YVDOT,V,VDOT,YDBR,YDSR 
C 

C  SURGE  CHARACTERISTICS 
C 
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REAL  U.UMAX.UD.N.RE.ETA.CDO.XPROP.XUDOT.XVR.XRR.XRDBR, 
&  X  R  DSR .  X  V  V  .X  VDB  R.X  VDSR ,  X  DB  RDB  R .  X  DS  RDSR  ,SIG  N  U ,  U  DOT 


C 

C  RUDDER  CHARACTERISTICS 
C 

REAL  DM  AX.DBR.DSR 
C 

C  SLIDING  MODE  CONTROL  VARIABLES 
C 

REAL  S1,EITA.SS1  ,SS2,SAT1  .SAT2.SS3.SAT3 

REAL  K11.K12,K13,K14,K15.K16.K21,K22,K23.K24.K25.K2(> 

REAL  SI  1 .S12,Si3,S14,S21  .S22.S23.S24 

( 

C  VEHICLE  CHARACTERISTICS 

C 

REAL  WT,L,XG.YG,IZ,RHO,G,M,X(18),H(18).NU 
C 

C  NAVIGATOR  VARIABLES 
C 

REAL  UCO.UC.VCO.VC 
REAL  TARGET. DAWAY, ALPHA 

REAL  XD.XD  1  ,XD2,X POS.XCl  R R.XCTE. YD.YD  1  ,YD2,  YPOS.YCL’ RR, YCTE 
REAL  XDOT.YDOT 
INTEGER  IWAY.IN AV 
C 

C  OBSERVER  VARIABLES 

C 

REAL  AI  I.AI2,A21,A22,BI  1.BI2.B2I.B22.L12.L23.L34.  VHAT  UCHAT. 

,V  VC  HAT, U COHAT. VCOHAT.Zl  .Z2.Z3.Z1  DOT.Z2DOT.Z3DOT.S1  .S2.S3 

C 

C  PROGRAM  VARIABLES 
C 

REAL  MM(3.3),MMINV(3,3),F(3),OUT(3) 

REAL  DELT 
REAL  PIE 
REAL  SGN 

REAL  V EC  1(  18), V EC2(  18) 

REAL  DRAGDX.DRAGXDX.UCF 
REAL  TIME,ARG1,ARG2,TALPHA 
INTEGER  I.ITER.ISCREEN.K.IOUT 
C 

YAW  HYDRC3DYN AMIC  COEFFICIENTS 

PARAMETER(NR=  -  1.640E-2,  NRDOT=-3.400E-3,  NV=  -7.420E-3, 

&  NVDOT=  1.240E-3,  NDBR=  1.290E-2,  NDSR=  -1.290E-2, 

&  CDY=  3.500E- 1) 

SWAY  HYDRODYNAMIC  COEFFICIENTS 
C 

PARAMETER(YR=  2.970E-2,  YRDOT=  1.240E-3,  YV=  -9.310E-2, 


185 


A:  YVD()T=-5.550E-2.  YDBR=  2.730E -  2.  YDSR=  2.730E-2) 

SI  RGE  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(1'Ma\=  6.0  .  X L! DOT  =  -7.580E-3,  XVR=  1  890E-2, 

X  XRR-  4.010E-3,  XRDBR=  8.180E-4.  X  RDSR=- 8. 180E  -  4, 

£  XV  V=  5.290E-2,  XVDBR=  1.730E-3,  XVDSR=  1.730E-3. 

£  XDBRDBR  =  -  1.010E-2,  XDSRDSR  =  - 1.010E-2) 

SLIDING  MODE  CONTROL  VARIABLES 

PARAMETERS  1  =  EOOOOO.Sl  2=  0 .00000  .S 1 3=  0 .00200  .S 1 4=0 .03030 . 

X  S2  1  =  0.00000, S22=  E()0000,S23=  1 .24230.824=0. 09200. 

X  K  1 1=  -  0.6207,  K 1 2=  -  7. 1470,  K 1 3=  -  3.8547.  K  14=  0.0000. 

X  K 15=  -4.44*49,  K  16=- 12.0714, 

X  K2  1=  1. 5414,  K22=  n. 3469,  K23=  3.2066.  K24=  0.0000. 

X  K25=  -4.6361,  K  26=  13.6038) 

REDDER  C  HA  R  ACTE  R  1ST  ICS 

PARAMETER  (DM  AX  =0.4) 

V  EH1CLE  CHAR  ACT  ERISTICS 

PA  RAM  ETER(WT  =  1 2000.,  L=17.425.  XCi=0.0,  YG=0.0.  IZ=  1 0000.. 

X  RHO=  1.940,  0=32.2.  NU=8.47E-4) 

NAVIGATOR  VARIABLES 

PA  R  A METERlTA RGET=  1 21.975) 

OBSERVER  VARIABLES 

PA R AMETER( A 1  1  =  -0.042 174797.  A  1  2=-0. 351578237. 


X 

X 

X 

X 

X 


A21  =  -0.002794897.  A22=- 0.098415870. 
B 1 1 =  0.012974539,  B12=  0.01  1513052, 
B2 1  =  0.004421618,  B22=-0.0042441 19, 
Sl=  -1.0,  S2=  -1.1, 

S3=  - 1.2) 


C 

C 

C 

C 

C 

C 


PROGRAM  VARIABLES 

PA  R  AMETER(PIE=3. 1 41593,DELT=0.01,ITER=50000) 

DEFINE  LENGTH  X  AND  HEIGHT  TERMS  FOR  THE  INTEGRATION 

X(l)=- 105.9/12. 

X(2)=- 104.3/12. 

X(3)=-99  3/12. 

X(4)=-94.3/ 1 2. 
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X(5)=-87..V  12. 

X(6)=- 76.8 ' 12. 

X(7)=-66.3;  12. 

X(8)=-55.8,  12. 

X(9)=72.7  12. 

X(  10)=79.2/  12. 

X  ( 1 1  )=83.2/  1 2. 

X  ( 1 2)=87.2  12. 

X(13)=91.2/12. 

X(  14)=9S.2  12. 

X(15)=99.2/12. 

X  ( 1 6)=1()1 .2.12 
X( 1 7)= 1 02. 1  12. 

X( 18)= 103.2  /  12. 

H(  1  )=().()/ 12. 

H(2)=2.28/12. 

H(3)=8.24/12. 

H(4)=13.96/ 12. 

H(5)= 19. 76/12. 

H(6)=25.10/ 12. 

H(7)=29. 36/12. 

H(8)=31. 85/12. 

H(9)=31. 8.5/12. 

H(10)=30. 00/12. 

H{1 1)=27. 84/12. 

H(12)=25. 12/12. 

H(1 31=21.44/ 12. 

H(14)=P.  12/12. 

H( 15)=12.00/ 12. 

H(16)=9. 12/12. 

H( 1 71=6.72/ 12. 

H(18)=0. 00/12. 

C 

C  DATA  ENTRY 

C 

WRITEC,*) 

10  WRITE!*, *)  ENTER  ORDERED  SPEED  IN  FT/SEC,  UD  = 
READ!*,*)  UD 

IF  (UD  LT.  0.0  .OR.  UD  ,GT.  UMAX)  THEN 
WRITE!*,*) 

WRITE!*,*)  ’  UD  MUST  BE:  0.0  <=  UD  <=  6.0’ 
WRITE!*,*)  ’  RE-ENTER  REALISTIC  SPEED  .  .  .’ 
WRITE!*,*) 

GO  TO  10 
ENDIF 
WRITE!*,*) 

WRITE!*,*)  ENTER  GLOBAL  CURRENTS,  UCO,VCO  =  ? 
READ(*,*)  UCO.VCO 

OPEN(UNIT=19,FILE=’CTECURR.DAT’,STATUS=’NEW’) 
WRITE(19,25)  UCO,VCO 
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FORMAT(F6.3,lX,F6.3) 

CLOSE(UNIl  =19) 

WRITE(V) 

30  WRITE(V)  ENTER  CONTROLLER  SI  =’ 

READ*’,*)  Si 
WRITE**.*) 

40  WRITE(V)  ’ENTER  CONTROL  LER  EITA  =  ’ 

READ(V)  EITA 
IF  (EITA  .LE.  0.0)  THEN 
WRITER, *) 

WRITE**,*)  ’EITA  MUST  BE:  EITA  >  0.0’ 

WRITE**,*)  RE-ENTER  REALISTIC  EITA’ 

WRITE**,*) 

GO  TO  40 
ENDIF 
WRITE**,*) 

WRITE**,*)  ’INITIAL  HEADING  IS  000  DEGREES  (DUE  NORTH)’ 
WRITE**,*)  ’INITIAL  POSITION  IS  (0.0, 0.0)’ 

WRITE**,*)  ENTER  FIRST  WAY-POINT:  X,Y  =  ?’ 

READ**,*)  XD,YD 

SET  INITIAL  STARTING  POSITION 


XD1=0.0 
YD1=0.0 
XD2=XD 
YD2=  VD 

OPEN(UNIT=20,FILE=’CTEWAYPT.DAT’,STATUS=’NEW) 
WRITE(20,*)  XD1.YD1 
WRITE(20,*)  XD2,YD2 

CALCULATE  THE  MASS 


M=WT/G 


SET  INITIAL  VALUES 

ALPHA=ATAN2((YD2- YD1),(XD2-  XD1)) 

UC=UCO*COS(ALPHA)+VCO*SIN(ALPHA) 

VC=VCO*COS(ALPHA)-UCO*SIN(ALPHA) 

U=UD 

N=UD/0.012 

DBR=0.0 

DSR=0.0 

V=0.0 

R=0.0 

PSI=0.0 

XPOS=0.0 

YPOS=0.0 

XCURR=XPOS 
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YCURR=YPOS 

XCTE=0.0 

YCTE=0.0 

TIME=0.0 

VHAT-0.0 

UCHAT=0.0 

VCHAT=0.0 

UCOHAT=0.0 

VCOHAT=0.0 

Z1=0.0 

Z2=0.0 

Z3=0.0 

Z1DOT=0.0 

Z2DOT=0.0 

Z3DOT=0.0 

CALCULATE  THE  MASS  MATRIX 

MM(1,1)=(M  -0.5*RHO*L*L*L*XUDOT) 

MM(1,2)=0.0 

MM(1,3)=-M*YG 

MM(2,1)=0.0 

MM(2,2)=(M  -0.5*RHO*L*L*L*YVDOT) 
MM(2,3)=(M*XG-0.5*RHO*L*L*L*L*YRDOT) 

MM(3,1)=-M*YG 

MM(3,2)=(M*XG-0.5*RHO*L*L*L*L*NVDOT) 

MM(3,3)=(IZ-0.5*RHO*L*L*L*L*L*NRDOT) 

INVERT  THE  MASS  MATRIX  USING  IMSL  LIBRARY  SUBROUTINE 

CALL  LINRG(3,MM,3,MMINV,3) 

INITIALIZE  THE  COUNTERS 

ISCREEN=1 

INAV=0 

IWAY=1 

IOUT=l 

OPEN  OUTPUT  FILES 

OPEN(UNIT=18,FILE=’CTEALLOUT.DAT’,STATUS=’NEW’) 

WRITE(18,*)  TIME,XPOS,YPOS,PSI,DBR,DSR,UD,U,V,VHAT,R,UC,UCHAT, 
&  VC,VCHAT 

SIMULATION  BEGINS 

DO  1000  1=1, ITER 

CALCULATE  THE  DRAG  FORCES 
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DO  50  K  =  l,18 

UCF=(V+R*X(K)) 

SGN  =  1.0 

IF  (UCF  .LT.  0.0)  SGN=-1.0 

VEC1(K)=H(K)*UCF*UCF*SGN 

VEC2(K)=H(K)*UCF*UCF*SGN*X(K) 

50  CONTINUE 

CALL  TRAP(18, VEC1,X,DRAGDX) 

CALL  TRAP(18, VEC2,X,DRAGXDX) 

C 

C  RHS  OF  SURGE  EQUATION 

C 

SIGNU=1.0 

IF  (  (U  .LT.  0.0)  .AND.  (N  .GT.  0.0)  )  SIGNU=-1.0 
IF  (  (U  .GT.  0.0)  .AND.  (N  .LT.  0.0)  )  SIGNU=-1.0 
RE=U*L/NU 

CD0=0. 00385  +  1.296E-  17*(RE-  1.2E7)*(RE  -  1.2E7) 

F(1)=0.5*RHO*L*L*U*  V*(X  VDBR*DBR  +  XVDSR*DSR) 

+  0.5*RHO*L*L*L*U*R*(XRDBR*DBR  +  XRDSR*DSR) 

+  0.5*RHO*L*L*X  VV*  V*  V 
+  (M*XG  +  0.5*RHO*L*L*L*L*XRR)*R*R 
+  (M  +  0.5*RHO*L*L*L*X  VR)*  V*R 

+  0.5*RHO*L*L*U*U*(XDBRDBR*DBR*DBR  +  XDSRDSR*DSR*DSR) 
+  0.5*RHO*L*L*CD0*(SIGNU*0.012*0.012*N*N  -  U*U) 

RHS  OF  SWAY  EQUATION 

F(2)=0.5*RHO*L*L*YV*U*V 

+  (0.5*RHO*L*L*L*YR  -  M)*U*R 
t  M*YG*R*R 

+  0.5*RHO*L*L*U*U*(YDBR*DBR  +  YDSR*DSR) 

-  0.5*RHO*CDY*DRAGDX 

RHS  OF  YAW  EQUATION 

F(3)=0.5*RHO*L*L*L*NV*U*V 

+  (0.5*RHO*L*L*L*L*NR*U  -  M*XG*U)*R 

-  M*YG*V*R 

+  0.5*RHO*L*L*L*U*U*(NDBR*DBR  +  NDSR’DSR) 

-  0.5*RHO*CDY*DRAGXDX 
C 

C  DEFINITION 

C 

PSIDOT=R 

C 

C  MULTIPLY  INVERTED  MASS  MATRIX  AND  F  VECTOR 

C 

CALL  MURR V(3,3,MMINV,3,3,F,l,3>OUT) 

UDOT=OUT(l) 
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VDOT=OUT(2) 

RDOT=OUT(3) 

XDOT=UCO+U*COS(PSI)-  V*SIN(PSI) 
YDOT=VCO+U*SIN(PSI)+V*COS(PSI) 

FIRST  ORDER  INTEGRATION 

U=U  +  DELT*UDOT 
V=V  +  DELT*VDOT 
R=R  +  DELT’RDOT 
PSI=PSI  +  DELT*PSIDOT 

MAKE  PSI  TO  BE:  -180  <  PSI  <=  180  DEGREES 

IF  (PSI  .GT.  PIE)  THEN 
PSI=PSI  -  2.0*PIE 
ELSEIF  (PSI  LE.  -PIE)  THEN 
PSI=PSI  +  2.0*PIE 
ENDIF 

XPOS=XPOS+DELT*XDOT 
YPOS=YPOS+DELT*YDOT 

CHECK  IF  TIME  FOR  NAV  UPDATE 

INA  V=INAV+1 
IF  (INAV  NE.  1)  GO  TO  80 
INAV=0 
XCURR=XPOS 
YCURR=YPOS 

DAWAY=SQRT((XCURR-  XD)**2  +  (YCURR- YD)**2) 

CHECK  DISTANCE  TO  WAY-POINT 

IF  (DAWAY  .LT.  TARGET)  THEN 

MISS  DISTANCE  TO  SCREEN,  ENTER  NEW  WAY-POINT, 
CALCULATE  NEW  ALPHA, UCHAT,VCHAT,XCTE,YCTE,Z2,Z3 

WRITE(*,*) 

WRITER, 60)  IWAY 
60  FORMATS  WAY-POINT  #’,I1,’  REACHED.’) 

WRITE(*,65)  DAWAY 

65  FORMATC  DAWAY  =  ’,F8.3,’  FEET’) 

UCOHAT=UCHAT*COS(ALPHA)-VCHAT*SIN(ALPHA) 
VCOHAT=VCHAT*COS(ALPHA)+UCHAT*SIN(  ALPHA) 
IWAY=IWAY+1 
WRITE(*,70)  IWAY 

70  FORMAT(’ ENTER  WAY-POINT  #’, II,’:  X,Y  =  ?’) 

READ(*,*)  XD,YD 
XD1=XD2 
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YD1=YD2 

XD2=XD 

YD2=YD 

WRITE(20,*)  XD2,YD2  4 

ALPHA=ATAN2((YD2- YD1),(XD2-XD1)) 

UC=UCO*COS(ALPHA)+VCO*SIN(ALPHA)  f 

VC=VCO*Cf  \LPHA)-  UCO*SIN(ALPHA) 

UCH  AT=UCOHAT*COS(A  LPHA)+VCOH  AT  *SIN(  ALPHA)  } 

VCHAT=VCOHAT*COS(ALPHA)-UCOHAT*SIN(ALPHA) 

XCTE=(YCURR- YD  1)*SIN(  ALPHA) 

&  +(XCURR-XDl)*COS(ALPHA) 

YCTE=(YCURR- YD  l)*COS(  ALPHA) 

&  -(XCURR-XD1)*SIN(ALPHA) 

Z2=VCHAT+S2*YCTE 

Z3=UCHAT+S3*XCTE+U*COS(PSI- ALPHA) 

ENDIF 


CALCULATE  NEW  XCTE,YCTE  DUE  TO  CHANGE  OF  XD,YD  AND/OR 
XCURR,YCURR 


)  XCTE=(YCURR-YDl)*SIN(ALPHA)+(XCURR-XDl)*COS(ALPHA) 

YCTE=(YCURR-YDl)*COS(ALPHA)-(XCURR-XDl)*SIN(ALPHA) 

******************************************************************************** 


OBSERVERS  FOR  SWAY  VELOCITY  AND  CURRENT  VELOCITIES 

*4^**4^*****  +  +  *  +  **4^********  +  ***************lt*******lt*]t*****  +  *4l*]t)t************** 


L12=(A1 1  *U-S1)/(A21*U) 

L23=-S2 

ZlDOT=Sl  *Z1  +  (A12*U-L12*A22*U+S1*L12)*R 
&  +  (B1 1  -B21*L12)*U*U*DBR  +  (B12-B22*L12)*U*U*DSR 

Z2DOT=S2*Zl  +  S2*Z2  -  L23*U*SIN(PSI- ALPHA)  +  S2*L12*R 
&  +  S2*L23*YCTE 

Z3DOT=S3*Z3  -  S3*S3*XCTE 


FIRST  ORDER  INTEGRATION 

Zl=Zl+DELT*ZlDOT 
Z2=Z2+DELT*Z2DOT 
Z3=Z3+DELT*Z3DOT 

FINAL  OBSERVER  EQUATIONS 

VHAT=L12*R  +  Z1 
VCHAT=L23*YCTE  +  Z2 

UCHAT=Z3  -  S3*XCTE  -  U*COS(PSI- ALPHA) 
£*******«******************•****♦*♦**♦*********************************♦****•**+* 
C  END  OBSERVERS 

£*****************+**********•*****++**+++*************************************** 

C 

C  CALCULATE  THE  CONTROL  LAWS 

C 
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'F  (VC/U  .GT.  1.0)  THEN 
ARG1  =  1.0 

ELSE1F  (VC/U  .LT.  -1.0)  THEN 
ARG1  =  - 1.0 

ELSE 

ARG1=VC/U 

ENDIF 

IF  ((PSI  .GE.  0.0  .AND.  PSI  .LE.  PIE)  .AND. 

&  (ALPHA  .GE.  -PIE  .AND.  ALPHA  .LE.  PSI-PIE))  THEN 

TALPHA=ALPHA  +  2.0*PIE 
ARG2=PSI-T  ALPHA 

ELSEIF  ((PSI  .GE.  -PIE  .AND.  PSI  .LT.  0.0)  .AND. 

&  (ALPHA  .GT.  PSI+PIE  .AND.  ALPHA  LE.  PIE))  THEN 

TALPHA= ALPHA  -  2.0*PIE 
ARG2=PSI-TALPHA 

ELSE 

TALPHA=ALPHA 

ARG2=PSI-TALPHA 

ENDIF 

SS1=S11*VHAT+S12*R+S13*ARG2+S14*YCTE+ 

&  ((SI/EITA)*((K13*K26-  K16*K23)/(K15*K26-  K16*K25))+S13)* 

&  ASIN(ARGl) 

SS2=S21*VHAT+S22*R+S23*ARG2+S24*YCTE+ 

&  ((SI/EITA)*((K15*  K23  -  K13*K25)/(K15*K26-  K16*K25))+S23)* 

&  ASIN(ARGl) 

SS3=U-UD 

IF  (ABS(SSl)  .LT.  SI)  SAT1=SS1/SI 
IF  (SSI  .LE.  -SI)  SAT1=- 1.0 
IF  (SSI  .GE.  SI)  SAT1=1.0 
IF  (ABS(SS2)  .LT.  SI)  SAT2=SS2/SI 
IF  (SS2  .LE.  -SI)  SAT2=- 1.0 
IF  (SS2  .GE.  SI)  SAT2=1.0 
IF  (ABS(SS3)  .LT.  SI)  SAT3=SS3/SI 
IF  (SS3  .LE.  -1.0)  SAT3=- 1.0 
IF  (SS3  .GE.  1.0)  SAT3=1.0 

DBR=K11*VHAT  +  K12*R  +  K13*ARG2  +  K14*YCTE 
&  +  EITA*K15*SAT1  +  EITA*  K16*SAT2 

DSR=K21*VHAT  +  K22*R  +  K23*ARG2  +  K24*YCTE 
&  +  EITA*  K25*S  ATI  +  EITA*  K26*SAT2 

N=-1153.9*SAT3  +  83.33*U 


CHECK  FOR  SATURATION 


IF  (DBR  .GT.  DMAX)  DBR=DMAX 
IF  (DBR  .LT.  -DMAX)  DBR=-DMAX 
IF  (DSR  .GT.  DMAX)  DSR=DM AX 
IF  (DSR  .LT.  -DMAX)  DSR=-DMAX 
IF  (N  .GE.  500.0)  N=500.0 
IF  (N  .LE.  -500.0)  N=- 500.0 
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C  OUTPUT 

C 

IF  (PSI  .LT.  0.0  .AND.  PSI  .GT.  -PIE)  THEN 

PSIX=(PSI+2.0*  PIE)  *180.0/ PIE  4 

ELSE 

PSIX=PSI*  180.0/ PIE  # 

ENDIF 

IF  (ISCREEN  .EQ.  200)  THEN 
TIME=I/100. 

WRITE(*,90)  TIME,PSIX,XPOS,YPOS 
90  FORMATf  TIME=  \F7.2,2X,’PSI=  \F9.4,2X, 

&  ’XPOS=  ’,F8.2,2X,’YPOS=  \F8.2) 

ISCREEN=0 

ENDIF 

IF  (IOUT  .EQ.  20)  THEN 
TIME=I/100. 

WRITE(18,*)  TIME, XPOS.YPOS, PSIX, DBR,DSR,UD,U, 

&  V,VHAT,R,UC,UCHAT,VC,VCHAT 

IOUT=0 
ENDIF 

ISCREEN=ISCREEN+1 
IOUT=IOUT+l 
1000  CONTINUE 

CLOSE(UNIT=18) 

CLOSE(UNIT=20) 

STOP 

END 

C 

C  NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 
C 

SUBROUTINE  TRAP(N,A,B,OUT) 

INTEGER  N,N1,I 

REAL  A(N),B(N),OUT,OUTl 

N1=N - 1 

OUT=0.0  , 

DO  10  1=1, N1 

OUT1  =  0.5*(A(I)  +  A(I+1))  *  (B(I+1)  -  B(I)) 

OUT  =  OUT  +  OUT1 
10  CONTINUE 
RETURN 
END 
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